mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-23 15:02:06 +08:00
dragonpilot 2022-11-30T09:37:39 for EON/C2
version: dragonpilot v0.9.1 beta for EON/C2 date: 2022-11-30T09:37:39 dp-dev(priv2) master commit: e2204c868277f20a263c2f93e88c42999d0b0c15
This commit is contained in:
Dragonpilot Team
committed by
Comma Device
Comma Device
parent
1fdc1202d1
commit
de34fa87ee
@@ -83,3 +83,4 @@ build/
|
||||
!**/.gitkeep
|
||||
|
||||
poetry.toml
|
||||
selfdrive/ui/_ui_nonav
|
||||
|
||||
@@ -1,5 +1,17 @@
|
||||
dragonpilot [Latest] - EON/C2 Release
|
||||
========================
|
||||
* Synced with openpilot master 2022.11.25 commits.
|
||||
* TWEAK: Ability to disable build-in NAV completely. (reduced CPU/memory load)
|
||||
* NEW: Lateral Controller Selector
|
||||
* Ability to select between DEFAULT / PID / LQR / Torque Controller.
|
||||
* NEW: Redesigned UI to match Comma's new UI/UX style.
|
||||
* NEW: Dynamic End-to-End can be turned on when lead car is going below x km/h. (Useful for stop and go traffic.)
|
||||
* NEW: Toggle to adapt Dynamic End-to-End to Following Dist. Mode.
|
||||
* NEW: Toggle to adapt Dynamic End-to-End to Accel Mode.
|
||||
* TWEAK: Bug fixes and improvement.
|
||||
|
||||
dragonpilot 2022.11.09 - EON/C2 Release
|
||||
========================
|
||||
* Synced with openpilot master 2022.10.26 commits.
|
||||
* COMMA: Torque controller used on all Toyota, Lexus, Hyundai, Kia, and Genesis models.
|
||||
* NEW: Mapd Database selector menu to use mapd offline located in software. When installing Database it will take some time. It is not stuck just be patience run `tmux a` for more info. DO NOT RESTART When database is being installed.
|
||||
@@ -75,6 +87,36 @@ dragonpilot 2022.07.29 - EON/C2 Release
|
||||
|
||||
dragonpilot [latest]
|
||||
=======================
|
||||
* Synced with openpilot master 2022.11.25 commits.
|
||||
* ADDED: Lateral Controller Selector.
|
||||
- Ability to select between DEFAULT / PID / LQR / Torque Controller.
|
||||
* ADDED: Redesigned UI to match Comma's new UI/UX style.
|
||||
* ADDED: Dynamic End-to-End can be turned on when lead car is going below x km/h. (Useful for stop and go traffic.)
|
||||
* ADDED: Toggle to adapt Dynamic End-to-End to Following Dist. Mode.
|
||||
* ADDED: Toggle to adapt Dynamic End-to-End to Accel Mode.
|
||||
* bug fix and improvement.
|
||||
|
||||
dragonpilot [2022.11.14]
|
||||
=======================
|
||||
* Synced with openpilot master 2022.11.14 commits.
|
||||
* COMMA: New Model.
|
||||
* COMMA: RAV4 TSS2 now uses PID.
|
||||
* REFACTORED: Turning off nav will free up some system resource. QR for nav has been move to home button / sidebar.
|
||||
* ADDED: 2 new toggle to turn on or off laneline / path and chevron.
|
||||
* ADDED: When reversing it will now show DM cam.
|
||||
|
||||
dragonpilot [2022.11.01]
|
||||
=======================
|
||||
* Synced with openpilot master 2022.11.01 commits.
|
||||
* COMMA: New Model.
|
||||
* ADDED: Multi Lateral Planner: Ability to choose different Lateral Planner (0 = LATEST, 1 = 0.8.13, 2 = 0.8.16).
|
||||
* READD: Toyota Prius ZSS support.
|
||||
* Improvement: Better User experience for installing mapd database.
|
||||
* Improvement: Turning on Lane Line will automatically will use DLP (Dynamic Lane Profile).
|
||||
* bug fix and improvement
|
||||
|
||||
dragonpilot [2022.10.26]
|
||||
=======================
|
||||
* Synced with openpilot master 2022.10.26 commits.
|
||||
* COMMA: Torque controller used on all Toyota, Lexus, Hyundai, Kia, and Genesis models.
|
||||
* COMMA: New Model with much improved e2e long.
|
||||
|
||||
+28
-9
@@ -1,21 +1,40 @@
|
||||
Version 0.8.17 (2022-XX-XX)
|
||||
Version 0.9.1 (2022-12-XX)
|
||||
========================
|
||||
* New driving model
|
||||
* Internal feature space accuracy increased tenfold during training, this makes the model dramatically more accurate.
|
||||
* New driver monitoring model
|
||||
* New end-to-end distracted trigger
|
||||
* Self-tuning torque lateral controller parameters
|
||||
* Parameters learned live for each car
|
||||
|
||||
|
||||
Version 0.9.0 (2022-11-21)
|
||||
========================
|
||||
* New driving model
|
||||
* Internal feature space information content increased tenfold during training to ~700 bits, which makes the model dramatically more accurate
|
||||
* Less reliance on previous frames makes model more reactive and snappy
|
||||
* Trained in new reprojective simulator
|
||||
* Trained in 36 hours from scratch, compared to one week for previous releases
|
||||
* Training now simulates both lateral and longitudinal behavior, which allows openpilot to slow down for turns, stop at traffic lights, and more in experimental mode
|
||||
* Experimental driving mode
|
||||
* End-to-end longitudinal control
|
||||
* Stops for traffic lights and stop signs
|
||||
* Slows down for turns
|
||||
* openpilot defaults to chill mode, enable experimental mode in settings
|
||||
* Driver monitoring updates
|
||||
* New bigger model with added end-to-end distracted trigger
|
||||
* Reduced false positives during driver calibration
|
||||
* Self-tuning torque controller: learns parameters live for each car
|
||||
* Torque controller used on all Toyota, Lexus, Hyundai, Kia, and Genesis models
|
||||
* UI updates
|
||||
* Multi-language in navigation
|
||||
* Matched speeds shown on car's dash
|
||||
* Multi-language in navigation
|
||||
* Improved update experience
|
||||
* Border turns grey while overriding steering
|
||||
* Added button to bookmark events while driving; view them later in comma connect
|
||||
* AGNOS 6
|
||||
* Bookmark events while driving; view them in comma connect
|
||||
* New onroad visualization for experimental mode
|
||||
* tools: new and improved cabana thanks to deanlee!
|
||||
* Experimental longitudinal support for Volkswagen, CAN-FD Hyundai, and new GM models
|
||||
* Genesis GV70 2022-23 support thanks to zunichky and sunnyhaibin!
|
||||
* Hyundai Santa Cruz 2021-22 support thanks to sunnyhaibin!
|
||||
* Kia Sportage 2023 support thanks to sunnyhaibin!
|
||||
* Kia Sportage Hybrid 2023 support thanks to sunnyhaibin!
|
||||
* Kia Stinger 2022 support thanks to sunnyhaibin!
|
||||
|
||||
Version 0.8.16 (2022-08-26)
|
||||
========================
|
||||
|
||||
+4
-3
@@ -172,6 +172,7 @@ struct CarState {
|
||||
# brake pedal, 0.0-1.0
|
||||
brake @5 :Float32; # this is user pedal only
|
||||
brakePressed @6 :Bool; # this is user pedal only
|
||||
regenBraking @45 :Bool; # this is user pedal only
|
||||
parkingBrake @39 :Bool;
|
||||
brakeHoldActive @38 :Bool;
|
||||
|
||||
@@ -219,9 +220,9 @@ struct CarState {
|
||||
charging @43 :Bool;
|
||||
|
||||
# dp
|
||||
cruiseActualEnabled @45 :Bool;
|
||||
engineRPM @46 :Float32;
|
||||
distanceLines @47:UInt8;
|
||||
cruiseActualEnabled @46 :Bool;
|
||||
engineRPM @47 :Float32;
|
||||
distanceLines @48:UInt8;
|
||||
|
||||
struct WheelSpeeds {
|
||||
# optional wheel speeds
|
||||
|
||||
+22
-16
@@ -24,20 +24,26 @@ struct DragonConf {
|
||||
dpUiEvent @16 :Bool;
|
||||
dpUiFace @17 :Bool;
|
||||
dpUiLeadInfo @18 :Bool;
|
||||
dpToyotaSng @19 :Bool;
|
||||
dpAccelProfileCtrl @20 :Bool;
|
||||
dpAccelProfile @21 :UInt8;
|
||||
dpToyotaCruiseOverride @22 :Bool;
|
||||
dpToyotaCruiseOverrideSpeed @23 :UInt8;
|
||||
dpToyotaAutoLock @24 :Bool;
|
||||
dpToyotaAutoUnlock @25 :Bool;
|
||||
dpMapd @26 :Bool;
|
||||
dpLocalDb @27 :Bool;
|
||||
dpDashcamd @28 :Bool;
|
||||
dpMazdaSteerAlert @29 :Bool;
|
||||
dpSpeedCheck @30 :Bool;
|
||||
dpFollowingProfileCtrl @31 :Bool;
|
||||
dpFollowingProfile @32 :UInt8;
|
||||
dpE2EConditional @33 :Bool;
|
||||
dpE2EConditionalAtSpeed @34 :UInt8;
|
||||
dpUiLaneline @19 :Bool;
|
||||
dpUiChevron @20 :Bool;
|
||||
dpUiDmCam @21 :Bool;
|
||||
dpToyotaSng @22 :Bool;
|
||||
dpAccelProfileCtrl @23 :Bool;
|
||||
dpAccelProfile @24 :UInt8;
|
||||
dpToyotaCruiseOverride @25 :Bool;
|
||||
dpToyotaCruiseOverrideSpeed @26 :UInt8;
|
||||
dpToyotaAutoLock @27 :Bool;
|
||||
dpToyotaAutoUnlock @28 :Bool;
|
||||
dpMapd @29 :Bool;
|
||||
dpLocalDb @30 :Bool;
|
||||
dpDashcamd @31 :Bool;
|
||||
dpMazdaSteerAlert @32 :Bool;
|
||||
dpSpeedCheck @33 :Bool;
|
||||
dpFollowingProfileCtrl @34 :Bool;
|
||||
dpFollowingProfile @35 :UInt8;
|
||||
dpE2EConditional @36 :Bool;
|
||||
dpE2EConditionalAtSpeed @37 :UInt8;
|
||||
dpE2EConditionalAtSpeedLead @38 :UInt8;
|
||||
dpE2EConditionalAdaptFp @39 :Bool;
|
||||
dpE2EConditionalAdaptAp @40 :Bool;
|
||||
}
|
||||
|
||||
Binary file not shown.
@@ -459,6 +459,8 @@ struct PandaState @0xa7649e2575e4591e {
|
||||
interruptRateClockSource @20;
|
||||
interruptRateTick @21;
|
||||
interruptRateExti @22;
|
||||
interruptRateSpi @23;
|
||||
interruptRateUart7 @24;
|
||||
# Update max fault type in boardd when adding faults
|
||||
}
|
||||
|
||||
@@ -472,6 +474,7 @@ struct PandaState @0xa7649e2575e4591e {
|
||||
dos @6;
|
||||
redPanda @7;
|
||||
redPandaV2 @8;
|
||||
tres @9;
|
||||
}
|
||||
|
||||
enum HarnessStatus {
|
||||
@@ -620,6 +623,8 @@ struct ControlsState @0x97ff69c53601abf1 {
|
||||
enabled @19 :Bool;
|
||||
active @36 :Bool;
|
||||
|
||||
experimentalMode @64 :Bool;
|
||||
|
||||
longControlState @30 :Car.CarControl.Actuators.LongControlState;
|
||||
vPid @2 :Float32;
|
||||
vTargetLead @3 :Float32;
|
||||
@@ -806,6 +811,9 @@ struct ModelDataV2 {
|
||||
|
||||
meta @12 :MetaData;
|
||||
|
||||
# Model perceived motion
|
||||
temporalPose @21 :Pose;
|
||||
|
||||
# All SI units and in device frame
|
||||
struct XYZTData {
|
||||
x @0 :List(Float32);
|
||||
@@ -869,6 +877,13 @@ struct ModelDataV2 {
|
||||
brake4MetersPerSecondSquaredProbs @5 :List(Float32);
|
||||
brake5MetersPerSecondSquaredProbs @6 :List(Float32);
|
||||
}
|
||||
|
||||
struct Pose {
|
||||
trans @0 :List(Float32); # m/s in device frame
|
||||
rot @1 :List(Float32); # rad/s in device frame
|
||||
transStd @2 :List(Float32); # std m/s in device frame
|
||||
rotStd @3 :List(Float32); # std rad/s in device frame
|
||||
}
|
||||
}
|
||||
|
||||
struct EncodeIndex {
|
||||
|
||||
Binary file not shown.
+13
-8
@@ -24,6 +24,7 @@ confs = [
|
||||
{'name': 'dp_jetson', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
|
||||
# nav service
|
||||
{'name': 'dp_nav', 'default': True, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_otisserv', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_nav_mapbox_token_pk', 'default': '', 'type': 'Text', 'conf_type': ['param']},
|
||||
{'name': 'dp_nav_mapbox_token_sk', 'default': '', 'type': 'Text', 'conf_type': ['param']},
|
||||
@@ -68,6 +69,9 @@ confs = [
|
||||
{'name': 'dp_ui_event', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_ui_face', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_ui_lead_info', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_ui_laneline', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_ui_chevron', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_ui_dm_cam', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
|
||||
#toyota
|
||||
{'name': 'dp_toyota_sng', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
@@ -84,16 +88,14 @@ confs = [
|
||||
{'name': 'dp_local_db', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
|
||||
|
||||
{'name': 'dp_lateral_lqr', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
|
||||
# dashcam related
|
||||
{'name': 'dp_dashcamd', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
# # auto shutdown
|
||||
{'name': 'dp_auto_shutdown', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_auto_shutdown_in', 'default': 90, 'type': 'UInt16', 'min': 0, 'max': 600, 'depends': [{'name': 'dp_auto_shutdown', 'vals': [True]}], 'conf_type': ['param']},
|
||||
{'name': 'dp_auto_shutdown', 'default': True, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_auto_shutdown_in', 'default': 60, 'type': 'UInt16', 'min': 0, 'max': 600, 'depends': [{'name': 'dp_auto_shutdown', 'vals': [True]}], 'conf_type': ['param']},
|
||||
|
||||
{'name': 'dp_mazda_steer_alert', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_mazda_dashcam_bypass', 'default': True, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_mazda_steer_alert', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_mazda_dashcam_bypass', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
# # service
|
||||
# {'name': 'dp_updated', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
# {'name': 'dp_logger', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
@@ -113,7 +115,7 @@ confs = [
|
||||
# {'name': 'dp_allow_gas', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_following_profile_ctrl', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_following_profile', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 2, 'depends': [{'name': 'dp_following_profile_ctrl', 'vals': [True]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_lateral_torque', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_lateral_tune', 'default': 0, 'type': 'UInt8', 'conf_type': ['param']},
|
||||
{'name': 'dp_lateral_torque_live_tune', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
# {'name': 'dp_accel_profile_ctrl', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
# {'name': 'dp_accel_profile', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 2, 'depends': [{'name': 'dp_accel_profile_ctrl', 'vals': [True]}], 'conf_type': ['param', 'struct']},
|
||||
@@ -133,10 +135,13 @@ confs = [
|
||||
# {'name': 'dp_ui_brightness', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 100, 'conf_type': ['param', 'struct']},
|
||||
# {'name': 'dp_ui_volume', 'default': -5, 'type': 'Int8', 'min': -5, 'max': 100, 'conf_type': ['param', 'struct']},
|
||||
# # toyota
|
||||
#{'name': 'dp_toyota_rav4_tss2_tune', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_toyota_rav4_tss2_tune', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_toyota_prius_bad_angle_tune', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
{'name': 'dp_e2e_conditional', 'default': False, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_e2e_conditional_at_speed', 'default': 60, 'type': 'UInt8', 'min': 30, 'max': 80, 'depends': [{'name': 'dp_e2e_conditional', 'vals': [True]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_e2e_conditional_at_speed_lead', 'default': 40, 'type': 'UInt8', 'min': 0, 'max': 80, 'depends': [{'name': 'dp_e2e_conditional', 'vals': [True]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_e2e_conditional_adapt_fp', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_e2e_conditional', 'vals': [True]}], 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_e2e_conditional_adapt_ap', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_e2e_conditional', 'vals': [True]}], 'conf_type': ['param', 'struct']},
|
||||
# {'name': 'dp_toyota_no_min_acc_limit', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
# {'name': 'dp_toyota_ldw', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
|
||||
{'name': 'dp_toyota_zss', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
|
||||
|
||||
@@ -8,6 +8,7 @@ supported_languages = {
|
||||
"main_zh-CHT": "zh-TW",
|
||||
"main_zh-CHS": "zh-CN",
|
||||
"main_ko": "ko-KR",
|
||||
"main_ja": "ja-JP",
|
||||
}
|
||||
|
||||
def events():
|
||||
|
||||
Binary file not shown.
@@ -86,6 +86,7 @@ int write_file(const char* path, const void* data, size_t size, int flags = O_WR
|
||||
FILE* safe_fopen(const char* filename, const char* mode);
|
||||
size_t safe_fwrite(const void * ptr, size_t size, size_t count, FILE * stream);
|
||||
int safe_fflush(FILE *stream);
|
||||
int safe_ioctl(int fd, unsigned long request, void *argp);
|
||||
|
||||
std::string readlink(const std::string& path);
|
||||
bool file_exists(const std::string& fn);
|
||||
|
||||
+1
-1
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "2022.11.02"
|
||||
#define COMMA_VERSION "2022.11.30"
|
||||
|
||||
+136
-131
@@ -4,23 +4,23 @@
|
||||
|
||||
A supported vehicle is one that just works when you install a comma three. All supported cars provide a better experience than any stock system.
|
||||
|
||||
# 211 Supported Cars
|
||||
# 215 Supported Cars
|
||||
|
||||
|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|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Audi|Q3 2020-21|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Audi|S3 2015-17|Adaptive Cruise Control (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|3 mph|6 mph|[](##)|[](##)|GM|
|
||||
|Chevrolet|Silverado 1500 2020-21|Safety Package II|Stock|3 mph|6 mph|[](##)|[](##)|GM|
|
||||
|Chevrolet|Volt 2017-18[<sup>1</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[](##)|[](##)|OBD-II|
|
||||
|Acura|RDX 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Cadillac|Escalade ESV 2016[<sup>3</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|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|GM|
|
||||
|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|GM|
|
||||
|Chevrolet|Volt 2017-18[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[](##)|[](##)|OBD-II|
|
||||
|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|FCA|
|
||||
|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|FCA|
|
||||
|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[](##)|[](##)|FCA|
|
||||
@@ -28,98 +28,102 @@ A supported vehicle is one that just works when you install a comma three. All s
|
||||
|Chrysler|Pacifica Hybrid 2019-22|Adaptive Cruise Control (ACC)|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|G70 2020|All|openpilot available[<sup>1</sup>](#footnotes)|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 (ACC)|openpilot|0 mph|6 mph|[](##)|[](##)|OBD-II|
|
||||
|GMC|Sierra 1500 2020-21|Driver Alert Package II|Stock|3 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|
|
||||
|Genesis|G90 2017-18|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
||||
|Genesis|GV70 2022-23|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|GMC|Acadia 2018[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[](##)|[](##)|OBD-II|
|
||||
|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|GM|
|
||||
|Honda|Accord 2018-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|Accord Hybrid 2018-22|All|openpilot available[<sup>1</sup>](#footnotes)|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 2019-21|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|2 mph[<sup>4</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 2017-21|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|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|CR-V 2017-22|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|e 2020|All|openpilot available[<sup>1</sup>](#footnotes)|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|Insight 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|Honda Bosch A|
|
||||
|Honda|Inspire 2018|All|openpilot available[<sup>1</sup>](#footnotes)|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)|Stock|19 mph|32 mph|[](##)|[](##)|Hyundai B|
|
||||
|Hyundai|Elantra 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
||||
|Hyundai|Elantra 2021-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
||||
|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai E|
|
||||
|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
||||
|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
||||
|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[](##)|[](##)|Hyundai J|
|
||||
|Hyundai|i30 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai E|
|
||||
|Hyundai|Ioniq 5 (with HDA II) 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai Q|
|
||||
|Hyundai|Ioniq 5 (without HDA II) 2022|Highway Driving Assist|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
||||
|Hyundai|Ioniq 5 (with HDA II) 2022-23|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai Q|
|
||||
|Hyundai|Ioniq 5 (without HDA II) 2022-23|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
||||
|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai C|
|
||||
|Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai C|
|
||||
|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot|0 mph|32 mph|[](##)|[](##)|Hyundai C|
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|32 mph|[](##)|[](##)|Hyundai C|
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2020-21|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai B|
|
||||
|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai B|
|
||||
|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|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)|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|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai I|
|
||||
|Hyundai|Palisade 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Hyundai|Santa Cruz 2021-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai N|
|
||||
|Hyundai|Santa Fe 2019-20|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai D|
|
||||
|Hyundai|Santa Fe 2021-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Hyundai|Santa Fe Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|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|Sonata 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
||||
|Hyundai|Sonata Hybrid 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
||||
|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Hyundai|Tucson Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|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 (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|FCA|
|
||||
|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|FCA|
|
||||
|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai E|
|
||||
|Kia|EV6 (with HDA II) 2022|Highway Driving Assist II|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai P|
|
||||
|Kia|EV6 (without HDA II) 2022|Highway Driving Assist|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai G|
|
||||
|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
||||
|Kia|Niro EV 2019|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Kia|Niro EV 2020|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai F|
|
||||
|Kia|Niro EV 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
||||
|Kia|Niro EV 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Kia|Niro Hybrid 2021|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai F|
|
||||
|Kia|Niro Hybrid 2022|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot|10 mph|32 mph|[](##)|[](##)|Hyundai C|
|
||||
|Kia|EV6 (with HDA II) 2022|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai P|
|
||||
|Kia|EV6 (without HDA II) 2022|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
||||
|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai G|
|
||||
|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
||||
|Kia|Niro EV 2019|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Kia|Niro EV 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai F|
|
||||
|Kia|Niro EV 2021|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
||||
|Kia|Niro EV 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Kia|Niro Hybrid 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai F|
|
||||
|Kia|Niro Hybrid 2022|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[<sup>1</sup>](#footnotes)|10 mph|32 mph|[](##)|[](##)|Hyundai C|
|
||||
|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai B|
|
||||
|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai G|
|
||||
|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
||||
|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
||||
|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
||||
|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai E|
|
||||
|Kia|Sportage Hybrid 2023|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai N|
|
||||
|Kia|Sportage 2023|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai N|
|
||||
|Kia|Sportage Hybrid 2023|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai N|
|
||||
|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|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|
|
||||
|Kia|Stinger 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
||||
|Kia|Telluride 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
||||
|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[<sup>2</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 2017-18|Lexus Safety System+|openpilot available[<sup>2</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 2018-19|All|openpilot available[<sup>2</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 2018-19|All|openpilot available[<sup>2</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|Lexus Safety System+|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX 2017-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX Hybrid 2016|Lexus Safety System+|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX Hybrid 2017-19|All|Stock[<sup>3</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Lexus|RX Hybrid 2017-19|All|openpilot available[<sup>2</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-23|All|Stock|0 mph|0 mph|[](##)|[](##)|Mazda|
|
||||
@@ -129,8 +133,8 @@ A supported vehicle is one that just works when you install a comma three. All s
|
||||
|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 (ACC)|Stock|0 mph|32 mph|[](##)|[](##)|Ram|
|
||||
|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|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|
|
||||
@@ -141,94 +145,95 @@ A supported vehicle is one that just works when you install a comma three. All s
|
||||
|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)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Karoq 2019-21[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Kodiaq 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Superb 2015-18|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Škoda|Kamiq 2021[<sup>6</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[<sup>9</sup>](#footnotes)|
|
||||
|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Škoda|Kodiaq 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[<sup>9</sup>](#footnotes)|
|
||||
|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|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 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Avalon 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Avalon 2019-21|All|openpilot available[<sup>2</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 2019-21|All|openpilot available[<sup>2</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 2018-20|All|Stock|0 mph[<sup>5</sup>](#footnotes)|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Camry 2021-22|All|openpilot|0 mph[<sup>5</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 2017-19|All|openpilot available[<sup>2</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 (Non-US only) 2020-23|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 2017-19|All|openpilot available[<sup>2</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 2017-19|All|openpilot available[<sup>2</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 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|Prius 2017-20|All|openpilot available[<sup>2</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 2017-20|All|openpilot available[<sup>2</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|Prius v 2017|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 2017-18|All|openpilot available[<sup>2</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 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[<sup>2</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)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Arteon eHybrid 2020-22[<sup>7,8</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Arteon R 2020-22[<sup>7,8</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Atlas 2018-23[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Atlas Cross Sport 2021-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|California 2021[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|31 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Caravelle 2020[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|31 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|CC 2018-22[<sup>7,8</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf 2015-20[<sup>8</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf R 2015-19[<sup>8</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Volkswagen|Jetta 2018-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Jetta GLI 2021-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Passat 2015-22[<sup>6,7,8</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Passat Alltrack 2015-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Passat GTE 2015-22[<sup>7,8</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Polo 2020-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Polo GTI 2020-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|T-Cross 2021[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|T-Roc 2021[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Taos 2022[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Teramont 2018-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Teramont Cross Sport 2021-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Teramont X 2021-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Tiguan 2019-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|Stock|0 mph|0 mph|[](##)|[](##)|VW|
|
||||
|Toyota|Sienna 2018-20|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
||||
|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Passat 2015-22[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Polo 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[<sup>9</sup>](#footnotes)|
|
||||
|Volkswagen|Polo GTI 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[<sup>9</sup>](#footnotes)|
|
||||
|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[<sup>9</sup>](#footnotes)|
|
||||
|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[<sup>9</sup>](#footnotes)|
|
||||
|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Tiguan 2019-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,8</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
||||
|
||||
<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 />
|
||||
<sup>2</sup>2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph. <br />
|
||||
<sup>3</sup>When the Driver Support Unit (DSU) is disconnected, openpilot Adaptive Cruise Control (ACC) will replace stock Adaptive Cruise Control (ACC). <b><i>NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).</i></b> <br />
|
||||
<sup>4</sup>openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
|
||||
<sup>5</sup>Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform. <br />
|
||||
<sup>6</sup>Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets. <br />
|
||||
<sup>7</sup>Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma store. Before ordering, remove the Lane Assist camera cover and check to see if the connector is black (older design) or light brown (newer design). In the interim, if your car has a J533 connector CAN gateway inside the dashboard, choose "VW J533 Development" from the vehicle drop-down for a suitable harness. (Some newer models are also observed to not have a J533 connector.) <br />
|
||||
<sup>8</sup>Includes versions with extra rear cargo space (may be called Variant, Estate, SportWagen, Shooting Brake, etc.) <br />
|
||||
<sup>1</sup>Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`. <br />
|
||||
<sup>2</sup>By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. If the Driver Support Unit (DSU) is disconnected, openpilot ACC will replace stock ACC. <b><i>NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).</i></b> <br />
|
||||
<sup>3</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 />
|
||||
<sup>4</sup>2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph. <br />
|
||||
<sup>5</sup>openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
|
||||
<sup>6</sup>Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform. <br />
|
||||
<sup>7</sup>Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets. <br />
|
||||
<sup>8</sup>Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC. <br />
|
||||
<sup>9</sup>Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot in software, but doesn't yet have a harness available from the comma store. <br />
|
||||
|
||||
## Community Maintained Cars
|
||||
Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/).
|
||||
|
||||
+17
-1
@@ -166,6 +166,11 @@ function two_init {
|
||||
echo "Installing $MODULE..."
|
||||
tar -zxvf "$LIB_PATH/$MODULE.tar.gz" -C "$PY_LIB_DEST/"
|
||||
fi
|
||||
# panda
|
||||
if [ ! -f "$PY_LIB_DEST/spidev.cpython-38.so" ]; then
|
||||
echo "Installing spidev.cpython-38.so..."
|
||||
cp -f "$LIB_PATH/spidev.cpython-38.so" "$PY_LIB_DEST/"
|
||||
fi
|
||||
mount -o remount,r /system
|
||||
|
||||
# Check for NEOS update
|
||||
@@ -216,7 +221,18 @@ function agnos_init {
|
||||
fi
|
||||
$DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST
|
||||
fi
|
||||
./custom_dep.py
|
||||
|
||||
#dp: change splash logo
|
||||
if [ -f "/usr/comma/.dp_splash" ]; then
|
||||
echo "DP splash exists."
|
||||
else
|
||||
echo "DP splash not deployed yet"
|
||||
sudo mount -o rw,remount /
|
||||
sudo cp /data/openpilot/selfdrive/dragonpilot/bg.jpg /usr/comma/bg.jpg
|
||||
sudo touch /usr/comma/.dp_splash
|
||||
sudo mount -o ro,remount /
|
||||
sudo reboot
|
||||
fi
|
||||
}
|
||||
|
||||
function launch {
|
||||
|
||||
+1
-1
@@ -11,7 +11,7 @@ if [ -z "$NEOS_VERSION" ]; then
|
||||
fi
|
||||
|
||||
if [ -z "$AGNOS_VERSION" ]; then
|
||||
export AGNOS_VERSION="6.1"
|
||||
export AGNOS_VERSION="6.2"
|
||||
fi
|
||||
|
||||
if [ -z "$PASSIVE" ]; then
|
||||
|
||||
+1
-1
@@ -5,7 +5,7 @@ Wondering what's the DBC file format? [Here](http://www.socialledge.com/sjsu/ind
|
||||
|
||||
## How to start reverse engineering cars
|
||||
|
||||
[opendbc](https://github.com/commaai/opendbc) is integrated with [cabana](https://community.comma.ai/cabana/).
|
||||
[opendbc](https://github.com/commaai/opendbc) is integrated with [cabana](https://github.com/commaai/openpilot/tree/master/tools/cabana).
|
||||
|
||||
Use [panda](https://github.com/commaai/panda) to connect your car to a computer.
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -71,7 +71,7 @@ BO_ 201 ECMEngineStatus: 8 K20_ECM
|
||||
SG_ EngineTPS : 39|8@0+ (0.392156863,0) [0|100.000000065] "%" NEO
|
||||
SG_ EngineRPM : 15|16@0+ (0.25,0) [0|0] "RPM" NEO
|
||||
SG_ CruiseMainOn : 29|1@0+ (1,0) [0|1] "" NEO
|
||||
SG_ Brake_Pressed : 40|1@0+ (1,0) [0|1] "" NEO
|
||||
SG_ BrakePressed : 40|1@0+ (1,0) [0|1] "" NEO
|
||||
SG_ Standstill : 2|1@0+ (1,0) [0|1] "" NEO
|
||||
SG_ CruiseActive : 31|2@0+ (1,0) [0|3] "" NEO
|
||||
|
||||
@@ -84,6 +84,7 @@ BO_ 209 EBCMBrakePedalSensors: 7 K17_EBCM
|
||||
SG_ BrakeNormalized2 : 47|8@0- (-1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 241 EBCMBrakePedalPosition: 6 K17_EBCM
|
||||
SG_ BrakePressed : 1|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BrakePedalPosition : 15|8@0+ (1,0) [0|255] "" NEO
|
||||
|
||||
BO_ 298 BCMDoorBeltStatus: 8 K9_BCM
|
||||
@@ -166,6 +167,9 @@ BO_ 501 ECMPRDNL2: 8 K20_ECM
|
||||
SG_ PRNDL2 : 27|4@0+ (1,0) [0|255] "" NEO
|
||||
SG_ ManualMode : 41|1@0+ (1,0) [0|1] "" NEO
|
||||
|
||||
BO_ 532 BRAKE_RELATED: 6 XXX
|
||||
SG_ UserBrakePressure : 0|9@0+ (1,0) [0|511] "" XXX
|
||||
|
||||
BO_ 560 EPBStatus: 8 EPB
|
||||
SG_ EPBClosed : 12|1@0+ (1,0) [0|1] "" NEO
|
||||
|
||||
@@ -186,6 +190,9 @@ BO_ 715 ASCMGasRegenCmd: 8 K124_ASCM
|
||||
|
||||
BO_ 717 ASCM_2CD: 5 K124_ASCM
|
||||
|
||||
BO_ 761 BRAKE_RELATED_2: 7 XXX
|
||||
SG_ UserBrakePressure2 : 47|9@0+ (1,0) [0|511] "" XXX
|
||||
|
||||
BO_ 789 EBCMFrictionBrakeCmd: 5 K124_ASCM
|
||||
SG_ RollingCounter : 33|2@0+ (1,0) [0|0] "" NEO
|
||||
SG_ FrictionBrakeMode : 7|4@0+ (1,0) [0|0] "" NEO
|
||||
@@ -203,17 +210,22 @@ BO_ 810 TCICOnStarGPSPosition: 8 K73_TCIC
|
||||
SG_ GPSLongitude : 39|32@0+ (1,-2147483648) [0|0] "milliarcsecond" NEO
|
||||
SG_ GPSLatitude : 7|32@0+ (1,0) [0|0] "milliarcsecond" NEO
|
||||
|
||||
BO_ 840 EBCMWheelSpdFront: 4 K17_EBCM
|
||||
BO_ 840 EBCMWheelSpdFront: 5 K17_EBCM
|
||||
SG_ FLWheelSpd : 7|16@0+ (0.0311,0) [0|255] "km/h" NEO
|
||||
SG_ FRWheelSpd : 23|16@0+ (0.0311,0) [0|255] "km/h" NEO
|
||||
|
||||
BO_ 842 EBCMWheelSpdRear: 5 K17_EBCM
|
||||
SG_ RLWheelSpd : 7|16@0+ (0.0311,0) [0|255] "km/h" NEO
|
||||
SG_ RRWheelSpd : 23|16@0+ (0.0311,0) [0|255] "km/h" NEO
|
||||
SG_ MovingForward : 32|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ MovingBackward : 33|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ MovingForward2 : 35|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ MovingBackward2 : 36|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 869 ASCM_365: 4 K124_ASCM
|
||||
|
||||
BO_ 880 ASCMActiveCruiseControlStatus: 6 K124_ASCM
|
||||
SG_ ACCCruiseState : 8|3@1+ (1,0) [0|7] "" XXX
|
||||
SG_ ACCLeadCar : 44|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ ACCAlwaysOne2 : 32|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ ACCAlwaysOne : 0|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
@@ -275,6 +287,8 @@ CM_ SG_ 352 Ignition "Non-zero when ignition is on";
|
||||
CM_ SG_ 451 GasPedalAndAcc2 "ACC baseline is 62";
|
||||
CM_ SG_ 497 Ignition "Describes ignition + preconditioning mode, noisy";
|
||||
CM_ SG_ 501 PRNDL2 "When ManualMode is Active, Value is 13=L1 12=L2 11=L3 ... 4=L10";
|
||||
CM_ SG_ 532 UserBrakePressure "can be lower than other brake position signals when the brakes are pre-filled from ACC braking and the user presses on the brakes. user-only pressure?";
|
||||
CM_ SG_ 761 UserBrakePressure2 "Similar to BRAKE_RELATED->UserBrakePressure";
|
||||
CM_ SG_ 1001 VehicleSpeed "Spinouts show here on 2wd. Speed derived from right front wheel (drive tire)";
|
||||
BA_DEF_ "UseGMParameterIDs" INT 0 0;
|
||||
BA_DEF_ "ProtocolType" STRING ;
|
||||
@@ -293,6 +307,7 @@ VAL_ 452 CruiseState 4 "Standstill" 3 "Faulted" 1 "Active" 0 "Off" ;
|
||||
VAL_ 309 PRNDL 3 "R" 2 "D" 1 "N" 0 "P" ;
|
||||
VAL_ 309 ESPButton 1 "Active" 0 "Inactive" ;
|
||||
VAL_ 384 LKASteeringCmdActive 1 "Active" 0 "Inactive" ;
|
||||
VAL_ 880 ACCCruiseState 2 "Adaptive" 3 "Adaptive" 4 "Non-adaptive" 5 "Non-adaptive" ;
|
||||
VAL_ 880 ACCLeadCar 1 "Present" 0 "Not Present" ;
|
||||
VAL_ 880 ACCGapLevel 3 "Far" 2 "Med" 1 "Near" 0 "Inactive" ;
|
||||
VAL_ 880 ACCResumeButton 1 "Pressed" 0 "Depressed" ;
|
||||
|
||||
@@ -91,7 +91,7 @@ BO_ 201 ECMEngineStatus: 8 K20_ECM
|
||||
SG_ EngineTPS : 39|8@0+ (0.392156863,0) [0|100.000000065] "%" NEO
|
||||
SG_ EngineRPM : 15|16@0+ (0.25,0) [0|0] "RPM" NEO
|
||||
SG_ CruiseMainOn : 29|1@0+ (1,0) [0|1] "" NEO
|
||||
SG_ Brake_Pressed : 40|1@0+ (1,0) [0|1] "" NEO
|
||||
SG_ BrakePressed : 40|1@0+ (1,0) [0|1] "" NEO
|
||||
SG_ Standstill : 2|1@0+ (1,0) [0|1] "" NEO
|
||||
SG_ CruiseActive : 31|2@0+ (1,0) [0|3] "" NEO
|
||||
|
||||
@@ -104,6 +104,7 @@ BO_ 209 EBCMBrakePedalSensors: 7 K17_EBCM
|
||||
SG_ BrakeNormalized2 : 47|8@0- (-1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 241 EBCMBrakePedalPosition: 6 K17_EBCM
|
||||
SG_ BrakePressed : 1|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BrakePedalPosition : 15|8@0+ (1,0) [0|255] "" NEO
|
||||
|
||||
BO_ 298 BCMDoorBeltStatus: 8 K9_BCM
|
||||
@@ -186,6 +187,9 @@ BO_ 501 ECMPRDNL2: 8 K20_ECM
|
||||
SG_ PRNDL2 : 27|4@0+ (1,0) [0|255] "" NEO
|
||||
SG_ ManualMode : 41|1@0+ (1,0) [0|1] "" NEO
|
||||
|
||||
BO_ 532 BRAKE_RELATED: 6 XXX
|
||||
SG_ UserBrakePressure : 0|9@0+ (1,0) [0|511] "" XXX
|
||||
|
||||
BO_ 560 EPBStatus: 8 EPB
|
||||
SG_ EPBClosed : 12|1@0+ (1,0) [0|1] "" NEO
|
||||
|
||||
@@ -206,6 +210,9 @@ BO_ 715 ASCMGasRegenCmd: 8 K124_ASCM
|
||||
|
||||
BO_ 717 ASCM_2CD: 5 K124_ASCM
|
||||
|
||||
BO_ 761 BRAKE_RELATED_2: 7 XXX
|
||||
SG_ UserBrakePressure2 : 47|9@0+ (1,0) [0|511] "" XXX
|
||||
|
||||
BO_ 789 EBCMFrictionBrakeCmd: 5 K124_ASCM
|
||||
SG_ RollingCounter : 33|2@0+ (1,0) [0|0] "" NEO
|
||||
SG_ FrictionBrakeMode : 7|4@0+ (1,0) [0|0] "" NEO
|
||||
@@ -223,17 +230,22 @@ BO_ 810 TCICOnStarGPSPosition: 8 K73_TCIC
|
||||
SG_ GPSLongitude : 39|32@0+ (1,-2147483648) [0|0] "milliarcsecond" NEO
|
||||
SG_ GPSLatitude : 7|32@0+ (1,0) [0|0] "milliarcsecond" NEO
|
||||
|
||||
BO_ 840 EBCMWheelSpdFront: 4 K17_EBCM
|
||||
BO_ 840 EBCMWheelSpdFront: 5 K17_EBCM
|
||||
SG_ FLWheelSpd : 7|16@0+ (0.0311,0) [0|255] "km/h" NEO
|
||||
SG_ FRWheelSpd : 23|16@0+ (0.0311,0) [0|255] "km/h" NEO
|
||||
|
||||
BO_ 842 EBCMWheelSpdRear: 5 K17_EBCM
|
||||
SG_ RLWheelSpd : 7|16@0+ (0.0311,0) [0|255] "km/h" NEO
|
||||
SG_ RRWheelSpd : 23|16@0+ (0.0311,0) [0|255] "km/h" NEO
|
||||
SG_ MovingForward : 32|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ MovingBackward : 33|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ MovingForward2 : 35|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ MovingBackward2 : 36|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 869 ASCM_365: 4 K124_ASCM
|
||||
|
||||
BO_ 880 ASCMActiveCruiseControlStatus: 6 K124_ASCM
|
||||
SG_ ACCCruiseState : 8|3@1+ (1,0) [0|7] "" XXX
|
||||
SG_ ACCLeadCar : 44|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ ACCAlwaysOne2 : 32|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
SG_ ACCAlwaysOne : 0|1@0+ (1,0) [0|0] "" Vector__XXX
|
||||
@@ -295,6 +307,8 @@ CM_ SG_ 352 Ignition "Non-zero when ignition is on";
|
||||
CM_ SG_ 451 GasPedalAndAcc2 "ACC baseline is 62";
|
||||
CM_ SG_ 497 Ignition "Describes ignition + preconditioning mode, noisy";
|
||||
CM_ SG_ 501 PRNDL2 "When ManualMode is Active, Value is 13=L1 12=L2 11=L3 ... 4=L10";
|
||||
CM_ SG_ 532 UserBrakePressure "can be lower than other brake position signals when the brakes are pre-filled from ACC braking and the user presses on the brakes. user-only pressure?";
|
||||
CM_ SG_ 761 UserBrakePressure2 "Similar to BRAKE_RELATED->UserBrakePressure";
|
||||
CM_ SG_ 1001 VehicleSpeed "Spinouts show here on 2wd. Speed derived from right front wheel (drive tire)";
|
||||
BA_DEF_ "UseGMParameterIDs" INT 0 0;
|
||||
BA_DEF_ "ProtocolType" STRING ;
|
||||
@@ -313,6 +327,7 @@ VAL_ 452 CruiseState 4 "Standstill" 3 "Faulted" 1 "Active" 0 "Off" ;
|
||||
VAL_ 309 PRNDL 3 "R" 2 "D" 1 "N" 0 "P" ;
|
||||
VAL_ 309 ESPButton 1 "Active" 0 "Inactive" ;
|
||||
VAL_ 384 LKASteeringCmdActive 1 "Active" 0 "Inactive" ;
|
||||
VAL_ 880 ACCCruiseState 2 "Adaptive" 3 "Adaptive" 4 "Non-adaptive" 5 "Non-adaptive" ;
|
||||
VAL_ 880 ACCLeadCar 1 "Present" 0 "Not Present" ;
|
||||
VAL_ 880 ACCGapLevel 3 "Far" 2 "Med" 1 "Near" 0 "Inactive" ;
|
||||
VAL_ 880 ACCResumeButton 1 "Pressed" 0 "Depressed" ;
|
||||
|
||||
+29
-20
@@ -196,33 +196,23 @@ BO_ 389 CAM_0x185: 8 CAMERA
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 416 CRUISE_INFO: 32 ADRV
|
||||
BO_ 416 SCC_CONTROL: 32 ADRV
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
SG_ NEW_SIGNAL_1 : 64|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ CRUISE_MAIN : 66|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ CRUISE_STATUS : 67|3@1+ (1,0) [0|7] "" XXX
|
||||
SG_ CRUISE_INACTIVE : 70|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ NEW_SIGNAL_4 : 103|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ NEW_SIGNAL_8 : 170|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ NEW_SIGNAL_10 : 156|5@0+ (1,0) [0|3] "" XXX
|
||||
SG_ ACCEL_REQ2 : 140|11@1+ (0.01,-10.23) [-10.23|10.24] "m/s^2" XXX
|
||||
SG_ ACCEL_REQ : 128|11@1+ (0.01,-10.23) [-10.23|10.24] "m/s^2" XXX
|
||||
SG_ ZEROS : 215|48@0+ (1,0) [0|281474976710655] "" XXX
|
||||
SG_ ZEROS_3 : 191|7@0+ (1,0) [0|127] "" XXX
|
||||
SG_ ZEROS_4 : 183|4@0+ (1,0) [0|63] "" XXX
|
||||
SG_ ZEROS_6 : 119|16@0+ (1,0) [0|65535] "" XXX
|
||||
SG_ ZEROS_7 : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_SPEED : 102|7@0+ (1,0) [0|127] "km/h or mph" XXX
|
||||
SG_ ZEROS_8 : 95|5@0+ (1,0) [0|31] "" XXX
|
||||
SG_ NEW_SIGNAL_3 : 109|2@0+ (1,0) [0|1] "" XXX
|
||||
SG_ NEW_SIGNAL_12 : 35|9@1+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_TMP_64 : 55|8@0+ (1,0) [0|63] "" XXX
|
||||
SG_ SET_ME_2 : 105|3@1+ (1,0) [0|7] "" XXX
|
||||
SG_ NEW_SIGNAL_6 : 104|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ ZEROS_9 : 71|5@1+ (1,0) [0|15] "" XXX
|
||||
SG_ ZEROS_10 : 111|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ NEW_SIGNAL_9 : 166|7@0+ (0.1,0) [0|63] "" XXX
|
||||
SG_ SET_ME_3 : 45|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ ObjValid : 46|1@0+ (1,0) [0|3] "" XXX
|
||||
SG_ NEW_SIGNAL_2 : 168|2@1+ (1,0) [0|3] "" XXX
|
||||
@@ -230,10 +220,19 @@ BO_ 416 CRUISE_INFO: 32 ADRV
|
||||
SG_ ACC_ObjDist : 24|11@1+ (0.1,0) [0|204.7] "m" XXX
|
||||
SG_ ZEROS_5 : 77|11@1+ (1,0) [0|2047] "" XXX
|
||||
SG_ DISTANCE_SETTING : 88|3@1+ (1,0) [0|3] "" XXX
|
||||
SG_ STOP_REQ : 184|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ NEW_SIGNAL_15 : 192|11@1+ (0.1,0) [0|204.7] "m" XXX
|
||||
SG_ ZEROS_2 : 207|5@0+ (1,0) [0|63] "" XXX
|
||||
SG_ CRUISE_STANDSTILL : 76|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ aReqRaw : 140|11@1+ (0.01,-10.23) [-10.23|10.24] "m/s^2" XXX
|
||||
SG_ aReqValue : 128|11@1+ (0.01,-10.23) [-10.23|10.24] "m/s^2" XXX
|
||||
SG_ ZEROS_7 : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ NEW_SIGNAL_4 : 158|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ ACCMode : 68|3@1+ (1,0) [0|7] "" XXX
|
||||
SG_ NEW_SIGNAL_12 : 35|9@1+ (0.1,0) [0|255] "" XXX
|
||||
SG_ JerkLowerLimit : 166|7@0+ (0.1,0) [0|12.7] "m/s^3" XXX
|
||||
SG_ StopReq : 184|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ NEW_SIGNAL_15 : 192|11@1+ (0.1,0) [0|204.7] "m" XXX
|
||||
SG_ VSetDis : 103|8@0+ (1,0) [0|255] "km/h or mph" XXX
|
||||
SG_ MainMode_ACC : 66|1@1+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 426 CRUISE_BUTTONS_ALT: 16 XXX
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
@@ -337,8 +336,8 @@ BO_ 507 CAM_0x1fb: 32 CAMERA
|
||||
BO_ 512 ADRV_0x200: 8 ADRV
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_3A : 32|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_E1 : 24|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_3A : 32|8@1+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 513 RADAR_0x201: 32 FRONT_RADAR
|
||||
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
|
||||
@@ -524,19 +523,29 @@ BO_ 485 BLINDSPOTS_FRONT_CORNER_1: 16 XXX
|
||||
|
||||
BO_ 506 CLUSTER_SPEED_LIMIT: 32 XXX
|
||||
SG_ SPEED_LIMIT_3 : 119|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SPEED_LIMIT_2 : 46|6@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SPEED_LIMIT_1 : 38|6@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SPEED_LIMIT_2 : 47|7@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SPEED_LIMIT_1 : 39|7@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SPEED_CHANGE_BLINKING : 129|1@1+ (1,0) [0|3] "" XXX
|
||||
SG_ CHIME_2 : 122|2@1+ (1,0) [0|7] "" XXX
|
||||
SG_ CHIME_1 : 133|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ARROW_DOWN : 120|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ARROW_UP : 121|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SECONDARY_LIMIT_1 : 79|8@0+ (1,0) [0|127] "" XXX
|
||||
SG_ SECONDARY_LIMIT_2 : 103|8@0+ (1,0) [0|127] "" XXX
|
||||
SG_ SCHOOL_ZONE : 155|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
|
||||
|
||||
CM_ SG_ 96 BRAKE_PRESSURE "User applied brake pedal pressure. Ramps from computer applied pressure on falling edge of cruise. Cruise cancels if !=0";
|
||||
CM_ SG_ 101 BRAKE_POSITION "User applied brake pedal position, max is ~700. Signed on some vehicles";
|
||||
CM_ SG_ 352 SET_ME_9 "has something to do with AEB settings";
|
||||
CM_ SG_ 373 PROBABLY_EQUIP "aeb equip?";
|
||||
CM_ SG_ 373 DriverBraking "Likely derived from BRAKE->BRAKE_POSITION";
|
||||
CM_ SG_ 373 DriverBrakingLowSens "Higher threshold version of DriverBraking";
|
||||
CM_ SG_ 373 PROBABLY_EQUIP "aeb equip?";
|
||||
CM_ SG_ 352 SET_ME_9 "has something to do with AEB settings";
|
||||
CM_ SG_ 416 VSetDis "set speed in display units";
|
||||
CM_ SG_ 961 COUNTER_ALT "only increments on change";
|
||||
CM_ SG_ 1041 COUNTER_ALT "only increments on change";
|
||||
CM_ SG_ 1043 COUNTER_ALT "only increments on change";
|
||||
|
||||
VAL_ 53 GEAR 0 "P" 5 "D" 6 "N" 7 "R" ;
|
||||
VAL_ 64 GEAR 0 "P" 5 "D" 6 "N" 7 "R" ;
|
||||
VAL_ 69 GEAR 0 "P" 5 "D" 6 "N" 7 "R" ;
|
||||
@@ -547,6 +556,6 @@ VAL_ 298 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green" ;
|
||||
VAL_ 298 LKA_MODE 1 "warning only" 2 "assist" 6 "off" ;
|
||||
VAL_ 304 GEAR 1 "P" 2 "R" 3 "N" 4 "D" ;
|
||||
VAL_ 352 AEB_SETTING 1 "off" 2 "warning only" 3 "active assist" ;
|
||||
VAL_ 416 CRUISE_STATUS 0 "cruise_inactive" 2 "cruise_active" 4 "driver_override" ;
|
||||
VAL_ 416 ACCMode 0 "off" 1 "enabled" 2 "driver_override" 3 "off_maybe_fault" 4 "cancelled" ;
|
||||
VAL_ 426 CRUISE_BUTTONS 0 "none" 1 "res_accel" 2 "set_decel" 3 "gap_distance" 4 "pause_resume" ;
|
||||
VAL_ 463 CRUISE_BUTTONS 0 "none" 1 "res_accel" 2 "set_decel" 3 "gap_distance" 4 "pause_resume" ;
|
||||
|
||||
+9
-9
@@ -1,10 +1,10 @@
|
||||
# flake8: noqa
|
||||
# pylint: skip-file
|
||||
from .python import Panda, PandaDFU, flash_release, \
|
||||
BASEDIR, ensure_st_up_to_date, PandaSerial, pack_can_buffer, unpack_can_buffer, \
|
||||
DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, MCU_TYPE_F4, DLC_TO_LEN, LEN_TO_DLC, \
|
||||
ALTERNATIVE_EXPERIENCE
|
||||
from .python import (Panda, PandaDFU, # noqa: F401
|
||||
BASEDIR, pack_can_buffer, unpack_can_buffer,
|
||||
DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, MCU_TYPE_F4, DLC_TO_LEN, LEN_TO_DLC,
|
||||
ALTERNATIVE_EXPERIENCE)
|
||||
|
||||
from .python.config import BOOTSTUB_ADDRESS, BLOCK_SIZE_FX, APP_ADDRESS_FX, \
|
||||
BLOCK_SIZE_H7, APP_ADDRESS_H7, DEVICE_SERIAL_NUMBER_ADDR_H7, \
|
||||
DEVICE_SERIAL_NUMBER_ADDR_FX
|
||||
from .python.serial import PandaSerial # noqa: F401
|
||||
|
||||
from .python.config import (BOOTSTUB_ADDRESS, BLOCK_SIZE_FX, APP_ADDRESS_FX, # noqa: F401
|
||||
BLOCK_SIZE_H7, APP_ADDRESS_H7, DEVICE_SERIAL_NUMBER_ADDR_H7, # noqa: F401
|
||||
DEVICE_SERIAL_NUMBER_ADDR_FX)
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+43
-33
@@ -8,18 +8,23 @@ import hashlib
|
||||
import datetime
|
||||
import traceback
|
||||
import warnings
|
||||
import logging
|
||||
from functools import wraps
|
||||
from typing import Optional
|
||||
from itertools import accumulate
|
||||
from .dfu import PandaDFU, MCU_TYPE_F2, MCU_TYPE_F4, MCU_TYPE_H7 # pylint: disable=import-error
|
||||
from .flash_release import flash_release # noqa pylint: disable=import-error
|
||||
from .update import ensure_st_up_to_date # noqa pylint: disable=import-error
|
||||
from .serial import PandaSerial # noqa pylint: disable=import-error
|
||||
from .isotp import isotp_send, isotp_recv # pylint: disable=import-error
|
||||
from .config import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, SECTOR_SIZES_FX, SECTOR_SIZES_H7 # noqa pylint: disable=import-error
|
||||
|
||||
from .config import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, SECTOR_SIZES_FX, SECTOR_SIZES_H7
|
||||
from .dfu import PandaDFU, MCU_TYPE_F2, MCU_TYPE_F4, MCU_TYPE_H7
|
||||
from .isotp import isotp_send, isotp_recv
|
||||
from .spi import SpiHandle
|
||||
|
||||
__version__ = '0.0.10'
|
||||
|
||||
# setup logging
|
||||
LOGLEVEL = os.environ.get('LOGLEVEL', 'INFO').upper()
|
||||
logging.basicConfig(level=LOGLEVEL, format='%(message)s')
|
||||
|
||||
|
||||
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
|
||||
|
||||
DEBUG = os.getenv("PANDADEBUG") is not None
|
||||
@@ -49,7 +54,7 @@ def pack_can_buffer(arr):
|
||||
snds.append(b'')
|
||||
idx += 1
|
||||
|
||||
#Apply counter to each 64 byte packet
|
||||
# Apply counter to each 64 byte packet
|
||||
for idx in range(len(snds)):
|
||||
tx = b''
|
||||
counter = 0
|
||||
@@ -167,6 +172,7 @@ class Panda:
|
||||
SERIAL_ESP = 1
|
||||
SERIAL_LIN1 = 2
|
||||
SERIAL_LIN2 = 3
|
||||
SERIAL_SOM_DEBUG = 4
|
||||
|
||||
GMLAN_CAN2 = 1
|
||||
GMLAN_CAN3 = 2
|
||||
@@ -183,6 +189,7 @@ class Panda:
|
||||
HW_TYPE_DOS = b'\x06'
|
||||
HW_TYPE_RED_PANDA = b'\x07'
|
||||
HW_TYPE_RED_PANDA_V2 = b'\x08'
|
||||
HW_TYPE_TRES = b'\x09'
|
||||
|
||||
CAN_PACKET_VERSION = 2
|
||||
HEALTH_PACKET_VERSION = 11
|
||||
@@ -192,10 +199,10 @@ class Panda:
|
||||
|
||||
F2_DEVICES = (HW_TYPE_PEDAL, )
|
||||
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, HW_TYPE_RED_PANDA_V2)
|
||||
H7_DEVICES = (HW_TYPE_RED_PANDA, HW_TYPE_RED_PANDA_V2, HW_TYPE_TRES)
|
||||
|
||||
INTERNAL_DEVICES = (HW_TYPE_UNO, HW_TYPE_DOS)
|
||||
HAS_OBD = (HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS, HW_TYPE_RED_PANDA, HW_TYPE_RED_PANDA_V2)
|
||||
HAS_OBD = (HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS, HW_TYPE_RED_PANDA, HW_TYPE_RED_PANDA_V2, HW_TYPE_TRES)
|
||||
|
||||
CLOCK_SOURCE_MODE_DISABLED = 0
|
||||
CLOCK_SOURCE_MODE_FREE_RUNNING = 1
|
||||
@@ -213,8 +220,8 @@ class Panda:
|
||||
FLAG_HYUNDAI_HYBRID_GAS = 2
|
||||
FLAG_HYUNDAI_LONG = 4
|
||||
FLAG_HYUNDAI_CAMERA_SCC = 8
|
||||
FLAG_HYUNDAI_CANFD_HDA2 = 8
|
||||
FLAG_HYUNDAI_CANFD_ALT_BUTTONS = 16
|
||||
FLAG_HYUNDAI_CANFD_HDA2 = 16
|
||||
FLAG_HYUNDAI_CANFD_ALT_BUTTONS = 32
|
||||
|
||||
FLAG_TESLA_POWERTRAIN = 1
|
||||
FLAG_TESLA_LONG_CONTROL = 2
|
||||
@@ -229,7 +236,7 @@ class Panda:
|
||||
FLAG_GM_HW_CAM = 1
|
||||
FLAG_GM_HW_CAM_LONG = 2
|
||||
|
||||
def __init__(self, serial: Optional[str] = None, claim: bool = True, disable_checks: bool = True):
|
||||
def __init__(self, serial: Optional[str] = None, claim: bool = True, spi: bool = False, disable_checks: bool = True):
|
||||
self._serial = serial
|
||||
self._disable_checks = disable_checks
|
||||
|
||||
@@ -237,9 +244,9 @@ class Panda:
|
||||
self._bcd_device = None
|
||||
|
||||
# connect and set mcu type
|
||||
self._spi = spi
|
||||
self.connect(claim)
|
||||
|
||||
|
||||
def close(self):
|
||||
self._handle.close()
|
||||
self._handle = None
|
||||
@@ -247,10 +254,30 @@ class Panda:
|
||||
def connect(self, claim=True, wait=False):
|
||||
if self._handle is not None:
|
||||
self.close()
|
||||
|
||||
context = usb1.USBContext()
|
||||
self._handle = None
|
||||
|
||||
if self._spi:
|
||||
self._handle = SpiHandle()
|
||||
|
||||
# TODO implement
|
||||
self._serial = "SPIDEV"
|
||||
self.bootstub = False
|
||||
|
||||
else:
|
||||
self.usb_connect(claim=claim, wait=wait)
|
||||
|
||||
assert self._handle is not None
|
||||
self._mcu_type = self.get_mcu_type()
|
||||
self.health_version, self.can_version, self.can_health_version = self.get_packets_versions()
|
||||
print("connected")
|
||||
|
||||
# disable openpilot's heartbeat checks
|
||||
if self._disable_checks:
|
||||
self.set_heartbeat_disabled()
|
||||
self.set_power_save(0)
|
||||
|
||||
def usb_connect(self, claim=True, wait=False):
|
||||
context = usb1.USBContext()
|
||||
while 1:
|
||||
try:
|
||||
for device in context.getDeviceList(skip_on_error=True):
|
||||
@@ -283,16 +310,6 @@ class Panda:
|
||||
break
|
||||
context = usb1.USBContext() # New context needed so new devices show up
|
||||
|
||||
assert self._handle is not None
|
||||
self._mcu_type = self.get_mcu_type()
|
||||
self.health_version, self.can_version, self.can_health_version = self.get_packets_versions()
|
||||
print("connected")
|
||||
|
||||
# disable openpilot's heartbeat checks
|
||||
if self._disable_checks:
|
||||
self.set_heartbeat_disabled()
|
||||
self.set_power_save(0)
|
||||
|
||||
def reset(self, enter_bootstub=False, enter_bootloader=False, reconnect=True):
|
||||
try:
|
||||
if enter_bootloader:
|
||||
@@ -583,9 +600,6 @@ class Panda:
|
||||
|
||||
# ******************* configuration *******************
|
||||
|
||||
def set_usb_power(self, on):
|
||||
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe6, int(on), 0, b'')
|
||||
|
||||
def set_power_save(self, power_save_enabled=0):
|
||||
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe7, int(power_save_enabled), 0, b'')
|
||||
|
||||
@@ -711,7 +725,7 @@ class Panda:
|
||||
def serial_write(self, port_number, ln):
|
||||
ret = 0
|
||||
for i in range(0, len(ln), 0x20):
|
||||
ret += self._handle.bulkWrite(2, struct.pack("B", port_number) + ln[i:i + 0x20])
|
||||
ret += self._handle.bulkWrite(2, struct.pack("B", port_number) + bytes(ln[i:i + 0x20], 'utf-8'))
|
||||
return ret
|
||||
|
||||
def serial_clear(self, port_number):
|
||||
@@ -834,7 +848,3 @@ class Panda:
|
||||
# ****************** Siren *****************
|
||||
def set_siren(self, enabled):
|
||||
self._handle.controlWrite(Panda.REQUEST_OUT, 0xf6, int(enabled), 0, b'')
|
||||
|
||||
# ****************** Debug *****************
|
||||
def set_green_led(self, enabled):
|
||||
self._handle.controlWrite(Panda.REQUEST_OUT, 0xf7, int(enabled), 0, b'')
|
||||
|
||||
@@ -1,74 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import time
|
||||
import requests
|
||||
import json
|
||||
import io
|
||||
|
||||
def flash_release(path=None, st_serial=None):
|
||||
from panda import Panda, PandaDFU
|
||||
from zipfile import ZipFile
|
||||
|
||||
def status(x):
|
||||
print("\033[1;32;40m" + x + "\033[00m")
|
||||
|
||||
if st_serial is not None:
|
||||
# look for Panda
|
||||
panda_list = Panda.list()
|
||||
if len(panda_list) == 0:
|
||||
raise Exception("panda not found, make sure it's connected and your user can access it")
|
||||
elif len(panda_list) > 1:
|
||||
raise Exception("Please only connect one panda")
|
||||
st_serial = panda_list[0]
|
||||
print("Using panda with serial %s" % st_serial)
|
||||
|
||||
if path is None:
|
||||
print("Fetching latest firmware from github.com/commaai/panda-artifacts")
|
||||
r = requests.get("https://raw.githubusercontent.com/commaai/panda-artifacts/master/latest.json")
|
||||
url = json.loads(r.text)['url']
|
||||
r = requests.get(url)
|
||||
print("Fetching firmware from %s" % url)
|
||||
path = io.BytesIO(r.content)
|
||||
|
||||
zf = ZipFile(path)
|
||||
zf.printdir()
|
||||
|
||||
version = zf.read("version").decode().strip()
|
||||
status("0. Preparing to flash " + str(version))
|
||||
|
||||
code_bootstub = zf.read("bootstub.panda.bin")
|
||||
code_panda = zf.read("panda.bin")
|
||||
|
||||
# enter DFU mode
|
||||
status("1. Entering DFU mode")
|
||||
panda = Panda(st_serial)
|
||||
panda.reset(enter_bootstub=True)
|
||||
panda.reset(enter_bootloader=True)
|
||||
time.sleep(1)
|
||||
|
||||
# program bootstub
|
||||
status("2. Programming bootstub")
|
||||
dfu = PandaDFU(PandaDFU.st_serial_to_dfu_serial(st_serial))
|
||||
dfu.program_bootstub(code_bootstub)
|
||||
time.sleep(1)
|
||||
|
||||
# flash main code
|
||||
status("3. Flashing main code")
|
||||
panda = Panda(st_serial)
|
||||
panda.flash(code=code_panda)
|
||||
panda.close()
|
||||
|
||||
# check for connection
|
||||
status("4. Verifying version")
|
||||
panda = Panda(st_serial)
|
||||
my_version = panda.get_version()
|
||||
print("dongle id: %s" % panda.get_serial()[0])
|
||||
print(my_version, "should be", version)
|
||||
assert(str(version) == str(my_version))
|
||||
|
||||
# done!
|
||||
status("6. Success!")
|
||||
|
||||
if __name__ == "__main__":
|
||||
flash_release(*sys.argv[1:])
|
||||
@@ -0,0 +1,105 @@
|
||||
import math
|
||||
import struct
|
||||
import spidev
|
||||
import logging
|
||||
from functools import reduce
|
||||
from typing import List
|
||||
|
||||
# Constants
|
||||
SYNC = 0x5A
|
||||
HACK = 0x79
|
||||
DACK = 0x85
|
||||
NACK = 0x1F
|
||||
CHECKSUM_START = 0xAB
|
||||
|
||||
MAX_RETRY_COUNT = 5
|
||||
|
||||
USB_MAX_SIZE = 0x40
|
||||
|
||||
# This mimics the handle given by libusb1 for easy interoperability
|
||||
class SpiHandle:
|
||||
def __init__(self):
|
||||
self.spi = spidev.SpiDev() # pylint: disable=c-extension-no-member
|
||||
self.spi.open(0, 0)
|
||||
|
||||
self.spi.max_speed_hz = 30000000
|
||||
|
||||
# helpers
|
||||
def _calc_checksum(self, data: List[int]) -> int:
|
||||
cksum = CHECKSUM_START
|
||||
for b in data:
|
||||
cksum ^= b
|
||||
return cksum
|
||||
|
||||
def _transfer(self, endpoint: int, data, max_rx_len: int = 1000) -> bytes:
|
||||
logging.debug("starting transfer: endpoint=%d, max_rx_len=%d", endpoint, max_rx_len)
|
||||
logging.debug("==============================================")
|
||||
|
||||
for n in range(MAX_RETRY_COUNT):
|
||||
logging.debug("\ntry #%d", n+1)
|
||||
try:
|
||||
logging.debug("- send header")
|
||||
packet = struct.pack("<BBHH", SYNC, endpoint, len(data), max_rx_len)
|
||||
packet += bytes([reduce(lambda x, y: x^y, packet) ^ CHECKSUM_START])
|
||||
self.spi.xfer2(packet)
|
||||
|
||||
logging.debug("- waiting for ACK")
|
||||
# TODO: add timeout?
|
||||
dat = b"\x00"
|
||||
while dat[0] not in [HACK, NACK]:
|
||||
dat = self.spi.xfer2(b"\x12")
|
||||
|
||||
if dat[0] == NACK:
|
||||
raise Exception("Got NACK response for header")
|
||||
|
||||
# send data
|
||||
logging.debug("- sending data")
|
||||
packet = bytes([*data, self._calc_checksum(data)])
|
||||
self.spi.xfer2(packet)
|
||||
|
||||
logging.debug("- waiting for ACK")
|
||||
dat = b"\x00"
|
||||
while dat[0] not in [DACK, NACK]:
|
||||
dat = self.spi.xfer2(b"\xab")
|
||||
|
||||
if dat[0] == NACK:
|
||||
raise Exception("Got NACK response for data")
|
||||
|
||||
# get response length, then response
|
||||
response_len_bytes = bytes(self.spi.xfer2(b"\x00" * 2))
|
||||
response_len = struct.unpack("<H", response_len_bytes)[0]
|
||||
|
||||
logging.debug("- receiving response")
|
||||
dat = bytes(self.spi.xfer2(b"\x00" * (response_len + 1)))
|
||||
if self._calc_checksum([DACK, *response_len_bytes, *dat]) != 0:
|
||||
raise Exception("SPI got bad checksum")
|
||||
|
||||
return dat[:-1]
|
||||
except Exception:
|
||||
logging.exception("SPI transfer failed, %d retries left", n)
|
||||
raise Exception(f"SPI transaction failed {MAX_RETRY_COUNT} times")
|
||||
|
||||
# libusb1 functions
|
||||
def close(self):
|
||||
self.spi.close()
|
||||
|
||||
def controlWrite(self, request_type: int, request: int, value: int, index: int, data, timeout: int = 0):
|
||||
return self._transfer(0, struct.pack("<BHHH", request, value, index, 0))
|
||||
|
||||
def controlRead(self, request_type: int, request: int, value: int, index: int, length: int, timeout: int = 0):
|
||||
return self._transfer(0, struct.pack("<BHHH", request, value, index, length))
|
||||
|
||||
# TODO: implement these properly
|
||||
def bulkWrite(self, endpoint: int, data: List[int], timeout: int = 0) -> int:
|
||||
for x in range(math.ceil(len(data) / USB_MAX_SIZE)):
|
||||
self._transfer(endpoint, data[USB_MAX_SIZE*x:USB_MAX_SIZE*(x+1)])
|
||||
return len(data)
|
||||
|
||||
def bulkRead(self, endpoint: int, length: int, timeout: int = 0) -> bytes:
|
||||
ret: List[int] = []
|
||||
for _ in range(math.ceil(length / USB_MAX_SIZE)):
|
||||
d = self._transfer(endpoint, [], max_rx_len=USB_MAX_SIZE)
|
||||
ret += d
|
||||
if len(d) < USB_MAX_SIZE:
|
||||
break
|
||||
return bytes(ret)
|
||||
@@ -1,44 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
|
||||
def ensure_st_up_to_date():
|
||||
from panda import Panda, PandaDFU, BASEDIR
|
||||
|
||||
with open(os.path.join(BASEDIR, "VERSION")) as f:
|
||||
repo_version = f.read()
|
||||
|
||||
repo_version += "-EON" if os.path.isfile('/EON') else "-DEV"
|
||||
|
||||
panda = None
|
||||
panda_dfu = None
|
||||
|
||||
while 1:
|
||||
# break on normal mode Panda
|
||||
panda_list = Panda.list()
|
||||
if len(panda_list) > 0:
|
||||
panda = Panda(panda_list[0])
|
||||
break
|
||||
|
||||
# flash on DFU mode Panda
|
||||
panda_dfu = PandaDFU.list()
|
||||
if len(panda_dfu) > 0:
|
||||
panda_dfu = PandaDFU(panda_dfu[0])
|
||||
panda_dfu.recover()
|
||||
|
||||
print("waiting for board...")
|
||||
time.sleep(1)
|
||||
|
||||
if panda.bootstub or not panda.get_version().startswith(repo_version):
|
||||
panda.flash()
|
||||
|
||||
if panda.bootstub:
|
||||
panda.recover()
|
||||
|
||||
assert(not panda.bootstub)
|
||||
version = str(panda.get_version())
|
||||
print("%s should be %s" % (version, repo_version))
|
||||
assert(version.startswith(repo_version))
|
||||
|
||||
if __name__ == "__main__":
|
||||
ensure_st_up_to_date()
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,4 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<svg viewBox="0 0 300 300" xmlns="http://www.w3.org/2000/svg">
|
||||
<path d="M 278.364 122.673 C 282.129 126.545 284.418 132.064 284.5 137.465 L 284.5 200.562 C 284.419 205.963 282.129 211.48 278.364 215.352 C 274.492 219.119 268.972 221.407 263.571 221.488 L 259.262 221.488 L 259.262 238.417 C 259.128 241.171 257.425 244.12 255.107 245.614 C 252.655 246.875 249.249 246.874 246.796 245.613 C 244.48 244.12 242.777 241.171 242.643 238.417 L 242.643 221.488 L 57.357 221.488 L 57.357 238.417 C 57.223 241.171 55.519 244.121 53.203 245.614 C 50.75 246.875 47.344 246.875 44.892 245.614 C 42.575 244.12 40.872 241.171 40.738 238.417 L 40.738 221.488 L 36.427 221.488 C 31.027 221.407 25.508 219.119 21.636 215.352 C 17.871 211.48 15.582 205.961 15.5 200.56 L 15.5 137.463 C 15.581 132.062 17.871 126.545 21.636 122.673 C 25.508 118.906 31.029 116.618 36.429 116.536 L 40.738 116.536 L 40.738 87.024 C 40.426 78.719 43.434 70.271 48.95 64.028 C 54.579 57.887 62.818 53.986 71.131 53.441 L 228.982 53.444 C 237.295 53.989 245.421 57.888 251.05 64.028 C 256.566 70.272 259.574 78.719 259.262 87.024 L 259.262 116.536 L 263.573 116.536 C 268.974 116.618 274.492 118.906 278.364 122.673 Z M 60.911 75.581 C 58.147 78.782 56.932 82.617 57.35 86.825 L 57.357 118.356 C 60.791 119.893 63.643 122.022 66.115 125.423 C 68.512 128.877 69.916 133.256 69.976 137.46 L 69.976 167.014 L 230.024 167.014 L 230.024 137.466 C 230.084 133.262 231.488 128.877 233.884 125.423 C 236.357 122.021 239.208 119.892 242.643 118.356 L 242.643 86.988 C 243.06 82.779 241.852 78.782 239.089 75.581 C 236.438 72.283 232.951 70.432 228.785 70.059 L 71.215 70.059 C 67.049 70.433 63.563 72.283 60.911 75.581 Z M 33.383 203.605 C 34.241 204.572 35.14 204.944 36.431 204.87 L 53.357 204.87 L 53.357 137.465 C 53.432 136.174 53.06 135.278 52.093 134.42 C 51.236 133.454 50.336 133.08 49.046 133.155 L 36.429 133.155 C 35.139 133.08 34.295 133.506 33.382 134.419 C 32.47 135.332 32.044 136.176 32.119 137.466 L 32.119 200.56 C 32.044 201.851 32.416 202.747 33.383 203.605 Z M 69.976 204.87 L 230.024 204.87 L 230.024 183.631 L 69.976 183.631 L 69.976 204.87 Z M 267.881 137.465 C 267.956 136.174 267.584 135.278 266.618 134.419 C 265.758 133.453 264.86 133.08 263.57 133.155 L 250.953 133.155 C 249.663 133.08 248.766 133.452 247.908 134.419 C 246.941 135.277 246.568 136.176 246.643 137.466 L 246.643 204.87 L 263.571 204.87 C 264.862 204.944 265.759 204.572 266.616 203.606 C 267.583 202.748 267.956 201.849 267.881 200.559 L 267.881 137.465 Z" fill="black" style="stroke-width: 4px; vector-effect: non-scaling-stroke; fill-opacity: 0.4;"/>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 2.6 KiB |
@@ -0,0 +1,10 @@
|
||||
<svg width="300" height="300" viewBox="0 0 300 300" fill="none" xmlns="http://www.w3.org/2000/svg">
|
||||
<path d="M182.019 181.218C123.429 239.83 67.543 258.769 54.9241 245.243C51.3182 241.636 51.3182 236.226 51.3182 231.717C51.3182 207.37 74.7548 160.481 124.331 114.489C129.74 109.98 129.74 101.864 125.233 96.454C120.725 91.0438 112.612 91.0438 107.204 95.5525C95.4866 106.373 85.5713 117.194 76.5569 128.016C51.3181 89.242 45.9096 61.2866 54.0213 52.2701C63.9367 42.351 100.894 50.4672 144.162 82.9291C157.683 92.8482 171.203 104.57 183.823 117.196C193.738 127.115 203.654 137.936 211.767 148.757C202.754 160.48 192.838 170.399 182.021 181.22L182.019 181.218ZM246.019 54.0753C255.033 63.0928 248.724 91.9498 225.287 128.918C217.174 118.999 209.062 109.982 200.048 100.063C191.034 91.0451 181.119 82.0274 171.204 73.9125C209.064 49.5654 237.006 45.0578 246.019 54.0753ZM264.048 36.04C243.316 15.3002 200.047 24.318 150.471 58.5842C100.894 24.317 56.7244 15.3005 35.9947 36.04C15.2627 56.7799 24.2771 99.1619 59.4313 149.66C38.6993 179.417 26.0793 208.272 26.0793 231.718C26.0793 247.949 32.3888 257.868 36.896 263.278C45.0091 271.394 54.9244 275 67.5433 275C105.403 275 158.583 241.635 200.049 200.157C209.063 191.14 218.077 181.22 225.288 171.301C249.625 209.176 255.034 237.128 246.02 246.144C238.809 253.357 218.076 250.653 193.739 238.028C187.43 235.322 180.218 237.127 176.613 243.438C173.008 249.75 175.712 256.965 182.022 260.571C200.951 270.49 218.979 274.999 232.501 274.999C245.121 274.999 255.036 271.391 263.148 263.277C283.88 242.537 274.866 199.252 240.612 149.657C272.163 105.473 286.583 59.4852 264.05 36.0397L264.048 36.04Z" fill="url(#paint0_radial_1352_397)"/>
|
||||
<path d="M175.414 154.169C175.414 171.6 161.29 185.729 143.866 185.729C126.441 185.729 112.317 171.6 112.317 154.169C112.317 136.738 126.441 122.609 143.866 122.609C161.29 122.609 175.414 136.738 175.414 154.169Z" fill="#E11C1C"/>
|
||||
<defs>
|
||||
<radialGradient id="paint0_radial_1352_397" cx="0" cy="0" r="1" gradientUnits="userSpaceOnUse" gradientTransform="translate(150 150) rotate(90) scale(125)">
|
||||
<stop offset="0.135417" stop-color="#E11C1C"/>
|
||||
<stop offset="1" stop-color="#FF8C22"/>
|
||||
</radialGradient>
|
||||
</defs>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 2.1 KiB |
@@ -0,0 +1,4 @@
|
||||
<svg width="300" height="300" viewBox="0 0 300 300" fill="none" xmlns="http://www.w3.org/2000/svg">
|
||||
<path d="M182.019 181.218C123.429 239.83 67.543 258.769 54.9241 245.243C51.3182 241.636 51.3182 236.226 51.3182 231.717C51.3182 207.37 74.7548 160.481 124.331 114.489C129.74 109.98 129.74 101.864 125.233 96.454C120.725 91.0438 112.612 91.0438 107.204 95.5525C95.4866 106.373 85.5713 117.194 76.5569 128.016C51.3181 89.242 45.9096 61.2866 54.0213 52.2701C63.9367 42.351 100.894 50.4672 144.162 82.9291C157.683 92.8482 171.203 104.57 183.823 117.196C193.738 127.115 203.654 137.936 211.767 148.757C202.754 160.48 192.838 170.399 182.021 181.22L182.019 181.218ZM246.019 54.0753C255.033 63.0928 248.724 91.9498 225.287 128.918C217.174 118.999 209.062 109.982 200.048 100.063C191.034 91.0451 181.119 82.0274 171.204 73.9125C209.064 49.5654 237.006 45.0578 246.019 54.0753ZM264.048 36.04C243.316 15.3002 200.047 24.318 150.471 58.5842C100.894 24.317 56.7244 15.3005 35.9947 36.04C15.2627 56.7799 24.2771 99.1619 59.4313 149.66C38.6993 179.417 26.0793 208.272 26.0793 231.718C26.0793 247.949 32.3888 257.868 36.896 263.278C45.0091 271.394 54.9244 275 67.5433 275C105.403 275 158.583 241.635 200.049 200.157C209.063 191.14 218.077 181.22 225.288 171.301C249.625 209.176 255.034 237.128 246.02 246.144C238.809 253.357 218.076 250.653 193.739 238.028C187.43 235.322 180.218 237.127 176.613 243.438C173.008 249.75 175.712 256.965 182.022 260.571C200.951 270.49 218.979 274.999 232.501 274.999C245.121 274.999 255.036 271.391 263.148 263.277C283.88 242.537 274.866 199.252 240.612 149.657C272.163 105.473 286.583 59.4852 264.05 36.0397L264.048 36.04Z" fill="black" fill-opacity="0.36"/>
|
||||
<path d="M175.414 154.169C175.414 171.6 161.29 185.729 143.866 185.729C126.441 185.729 112.317 171.6 112.317 154.169C112.317 136.738 126.441 122.609 143.866 122.609C161.29 122.609 175.414 136.738 175.414 154.169Z" fill="black" fill-opacity="0.36"/>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 1.9 KiB |
@@ -0,0 +1,4 @@
|
||||
<svg width="300" height="300" viewBox="0 0 300 300" fill="none" xmlns="http://www.w3.org/2000/svg">
|
||||
<path d="M182.019 181.218C123.429 239.83 67.543 258.769 54.9241 245.243C51.3182 241.636 51.3182 236.226 51.3182 231.717C51.3182 207.37 74.7548 160.481 124.331 114.489C129.74 109.98 129.74 101.864 125.233 96.454C120.725 91.0438 112.612 91.0438 107.204 95.5525C95.4866 106.373 85.5713 117.194 76.5569 128.016C51.3181 89.242 45.9096 61.2866 54.0213 52.2701C63.9367 42.351 100.894 50.4672 144.162 82.9291C157.683 92.8482 171.203 104.57 183.823 117.196C193.738 127.115 203.654 137.936 211.767 148.757C202.754 160.48 192.838 170.399 182.021 181.22L182.019 181.218ZM246.019 54.0753C255.033 63.0928 248.724 91.9498 225.287 128.918C217.174 118.999 209.062 109.982 200.048 100.063C191.034 91.0451 181.119 82.0274 171.204 73.9125C209.064 49.5654 237.006 45.0578 246.019 54.0753ZM264.048 36.04C243.316 15.3002 200.047 24.318 150.471 58.5842C100.894 24.317 56.7244 15.3005 35.9947 36.04C15.2627 56.7799 24.2771 99.1619 59.4313 149.66C38.6993 179.417 26.0793 208.272 26.0793 231.718C26.0793 247.949 32.3888 257.868 36.896 263.278C45.0091 271.394 54.9244 275 67.5433 275C105.403 275 158.583 241.635 200.049 200.157C209.063 191.14 218.077 181.22 225.288 171.301C249.625 209.176 255.034 237.128 246.02 246.144C238.809 253.357 218.076 250.653 193.739 238.028C187.43 235.322 180.218 237.127 176.613 243.438C173.008 249.75 175.712 256.965 182.022 260.571C200.951 270.49 218.979 274.999 232.501 274.999C245.121 274.999 255.036 271.391 263.148 263.277C283.88 242.537 274.866 199.252 240.612 149.657C272.163 105.473 286.583 59.4852 264.05 36.0397L264.048 36.04Z" fill="white"/>
|
||||
<path d="M175.414 154.169C175.414 171.6 161.29 185.729 143.866 185.729C126.441 185.729 112.317 171.6 112.317 154.169C112.317 136.738 126.441 122.609 143.866 122.609C161.29 122.609 175.414 136.738 175.414 154.169Z" fill="white"/>
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 1.8 KiB |
Binary file not shown.
Binary file not shown.
@@ -93,7 +93,6 @@ class Panda {
|
||||
void send_heartbeat(bool engaged);
|
||||
void set_can_speed_kbps(uint16_t bus, uint16_t speed);
|
||||
void set_data_speed_kbps(uint16_t bus, uint16_t speed);
|
||||
void set_canfd_non_iso(uint16_t bus, bool non_iso);
|
||||
void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
|
||||
bool can_receive(std::vector<can_frame>& out_vec);
|
||||
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
#pragma once
|
||||
|
||||
#include <mutex>
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
#include <linux/spi/spidev.h>
|
||||
|
||||
#include <libusb-1.0/libusb.h>
|
||||
|
||||
|
||||
#define TIMEOUT 0
|
||||
#define SPI_BUF_SIZE 1024
|
||||
|
||||
const bool PANDA_NO_RETRY = getenv("PANDA_NO_RETRY");
|
||||
|
||||
|
||||
// comms base class
|
||||
class PandaCommsHandle {
|
||||
public:
|
||||
PandaCommsHandle(std::string serial) {};
|
||||
virtual ~PandaCommsHandle() {};
|
||||
virtual void cleanup() = 0;
|
||||
|
||||
std::atomic<bool> connected = true;
|
||||
std::atomic<bool> comms_healthy = true;
|
||||
static std::vector<std::string> list();
|
||||
|
||||
// HW communication
|
||||
virtual int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT) = 0;
|
||||
virtual int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT) = 0;
|
||||
virtual int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0;
|
||||
virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0;
|
||||
|
||||
protected:
|
||||
std::recursive_mutex hw_lock;
|
||||
};
|
||||
|
||||
class PandaUsbHandle : public PandaCommsHandle {
|
||||
public:
|
||||
PandaUsbHandle(std::string serial);
|
||||
~PandaUsbHandle();
|
||||
int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT);
|
||||
int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT);
|
||||
int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
|
||||
int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
|
||||
void cleanup();
|
||||
|
||||
static std::vector<std::string> list();
|
||||
|
||||
private:
|
||||
libusb_context *ctx = NULL;
|
||||
libusb_device_handle *dev_handle = NULL;
|
||||
std::vector<uint8_t> recv_buf;
|
||||
void handle_usb_issue(int err, const char func[]);
|
||||
};
|
||||
|
||||
class PandaSpiHandle : public PandaCommsHandle {
|
||||
public:
|
||||
PandaSpiHandle(std::string serial);
|
||||
~PandaSpiHandle();
|
||||
int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT);
|
||||
int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT);
|
||||
int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
|
||||
int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
|
||||
void cleanup();
|
||||
|
||||
static std::vector<std::string> list();
|
||||
|
||||
private:
|
||||
int spi_fd = -1;
|
||||
uint8_t tx_buf[SPI_BUF_SIZE];
|
||||
uint8_t rx_buf[SPI_BUF_SIZE];
|
||||
|
||||
int wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack);
|
||||
int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len);
|
||||
int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len);
|
||||
int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len);
|
||||
};
|
||||
+95
@@ -0,0 +1,95 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import random
|
||||
import time
|
||||
import unittest
|
||||
from collections import defaultdict
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import car
|
||||
from common.params import Params
|
||||
from common.spinner import Spinner
|
||||
from common.timeout import Timeout
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.car import make_can_msg
|
||||
from system.hardware import TICI
|
||||
from selfdrive.test.helpers import phone_only, with_processes
|
||||
|
||||
|
||||
class TestBoardd(unittest.TestCase):
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
os.environ['STARTED'] = '1'
|
||||
os.environ['BOARDD_LOOPBACK'] = '1'
|
||||
cls.spinner = Spinner()
|
||||
|
||||
@classmethod
|
||||
def tearDownClass(cls):
|
||||
cls.spinner.close()
|
||||
|
||||
@phone_only
|
||||
@with_processes(['pandad'])
|
||||
def test_loopback(self):
|
||||
# wait for boardd to init
|
||||
time.sleep(2)
|
||||
|
||||
with Timeout(60, "boardd didn't start"):
|
||||
sm = messaging.SubMaster(['pandaStates'])
|
||||
while sm.rcv_frame['pandaStates'] < 1 and len(sm['pandaStates']) == 0:
|
||||
sm.update(1000)
|
||||
|
||||
num_pandas = len(sm['pandaStates'])
|
||||
if TICI:
|
||||
self.assertGreater(num_pandas, 1, "connect another panda for multipanda tests")
|
||||
|
||||
# boardd blocks on CarVin and CarParams
|
||||
cp = car.CarParams.new_message()
|
||||
|
||||
safety_config = car.CarParams.SafetyConfig.new_message()
|
||||
safety_config.safetyModel = car.CarParams.SafetyModel.allOutput
|
||||
cp.safetyConfigs = [safety_config]*num_pandas
|
||||
|
||||
params = Params()
|
||||
params.put("CarVin", b"0"*17)
|
||||
params.put_bool("ControlsReady", True)
|
||||
params.put("CarParams", cp.to_bytes())
|
||||
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
can = messaging.sub_sock('can', conflate=False, timeout=100)
|
||||
time.sleep(0.2)
|
||||
|
||||
n = 200
|
||||
for i in range(n):
|
||||
self.spinner.update(f"boardd loopback {i}/{n}")
|
||||
|
||||
sent_msgs = defaultdict(set)
|
||||
for _ in range(random.randrange(10)):
|
||||
to_send = []
|
||||
for __ in range(random.randrange(100)):
|
||||
bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3])
|
||||
addr = random.randrange(1, 1<<29)
|
||||
dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9)))
|
||||
sent_msgs[bus].add((addr, dat))
|
||||
to_send.append(make_can_msg(addr, dat, bus))
|
||||
sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan'))
|
||||
|
||||
for _ in range(100 * 2):
|
||||
recvd = messaging.drain_sock(can, wait_for_one=True)
|
||||
for msg in recvd:
|
||||
for m in msg.can:
|
||||
if m.src >= 128:
|
||||
key = (m.address, m.dat)
|
||||
assert key in sent_msgs[m.src-128], f"got unexpected msg: {m.src=} {m.address=} {m.dat=}"
|
||||
sent_msgs[m.src-128].discard(key)
|
||||
|
||||
if all(len(v) == 0 for v in sent_msgs.values()):
|
||||
break
|
||||
|
||||
# if a set isn't empty, messages got dropped
|
||||
for bus in sent_msgs.keys():
|
||||
assert not len(sent_msgs[bus]), f"loop {i}: bus {bus} missing {len(sent_msgs[bus])} messages"
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -26,6 +26,12 @@ CAR_INFO: Dict[str, CarInfo] = {
|
||||
CAR.BODY: CarInfo("comma body", package="All"),
|
||||
}
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.BODY: [{
|
||||
513: 8, 516: 8, 514: 3, 515: 4,
|
||||
}],
|
||||
}
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
requests=[
|
||||
Request(
|
||||
@@ -40,10 +46,13 @@ FW_VERSIONS = {
|
||||
CAR.BODY: {
|
||||
(Ecu.engine, 0x720, None): [
|
||||
b'0.0.01',
|
||||
b'02/27/2022'
|
||||
b'02/27/2022',
|
||||
b'0.3.00a',
|
||||
],
|
||||
# git hash of the firmware used
|
||||
(Ecu.debug, 0x721, None): [
|
||||
b'166bd860' # git hash of the firmware used
|
||||
b'166bd860',
|
||||
b'dc780f85',
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
@@ -241,6 +241,7 @@ FW_VERSIONS = {
|
||||
b'68334977AH',
|
||||
b'68504022AB',
|
||||
b'68530686AB',
|
||||
b'68504022AC',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672895AB',
|
||||
|
||||
@@ -68,7 +68,17 @@ class Harness(Enum):
|
||||
none = "None"
|
||||
|
||||
|
||||
CarFootnote = namedtuple("CarFootnote", ["text", "column"], defaults=[None])
|
||||
CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only"], defaults=(None, False))
|
||||
|
||||
|
||||
class CommonFootnote(Enum):
|
||||
EXP_LONG_AVAIL = CarFootnote(
|
||||
"Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`. ",
|
||||
Column.LONGITUDINAL, docs_only=True)
|
||||
EXP_LONG_DSU = CarFootnote(
|
||||
"By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. If the Driver Support Unit (DSU) is disconnected, openpilot ACC will replace " +
|
||||
"stock ACC. <b><i>NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).</i></b>",
|
||||
Column.LONGITUDINAL)
|
||||
|
||||
|
||||
def get_footnotes(footnotes: List[Enum], column: Column) -> List[Enum]:
|
||||
@@ -128,11 +138,22 @@ class CarInfo:
|
||||
self.car_name = CP.carName
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.make, self.model, self.years = split_name(self.name)
|
||||
|
||||
op_long = "Stock"
|
||||
if CP.openpilotLongitudinalControl and not CP.enableDsu:
|
||||
op_long = "openpilot"
|
||||
elif CP.experimentalLongitudinalAvailable or CP.enableDsu:
|
||||
op_long = "openpilot available"
|
||||
if CP.enableDsu:
|
||||
self.footnotes.append(CommonFootnote.EXP_LONG_DSU)
|
||||
else:
|
||||
self.footnotes.append(CommonFootnote.EXP_LONG_AVAIL)
|
||||
|
||||
self.row = {
|
||||
Column.MAKE: self.make,
|
||||
Column.MODEL: self.model,
|
||||
Column.PACKAGE: self.package,
|
||||
Column.LONGITUDINAL: "openpilot" if CP.openpilotLongitudinalControl or CP.experimentalLongitudinalAvailable else "Stock",
|
||||
Column.LONGITUDINAL: op_long,
|
||||
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,
|
||||
@@ -151,6 +172,9 @@ class CarInfo:
|
||||
|
||||
return self
|
||||
|
||||
def init_make(self, CP: car.CarParams):
|
||||
"""CarInfo subclasses can add make-specific logic for harness selection, footnotes, etc."""
|
||||
|
||||
def get_detail_sentence(self, CP):
|
||||
if not CP.notCar:
|
||||
sentence_builder = "openpilot upgrades your <strong>{car_model}</strong> with automated lane centering{alc} and adaptive cruise control{acc}."
|
||||
|
||||
@@ -61,10 +61,10 @@ class FordCarInfo(CarInfo):
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
|
||||
CAR.ESCAPE_MK4: [
|
||||
FordCarInfo("Ford Escape 2020"),
|
||||
FordCarInfo("Ford Kuga EU", "Driver Assistance Pack"),
|
||||
FordCarInfo("Ford Escape 2020-21"),
|
||||
FordCarInfo("Ford Kuga 2020-21", "Driver Assistance Pack"),
|
||||
],
|
||||
CAR.EXPLORER_MK6: FordCarInfo("Ford Explorer 2020-21"),
|
||||
CAR.EXPLORER_MK6: FordCarInfo("Ford Explorer 2020-22"),
|
||||
CAR.FOCUS_MK4: FordCarInfo("Ford Focus EU 2019", "Driver Assistance Pack"),
|
||||
}
|
||||
|
||||
@@ -87,31 +87,41 @@ FW_QUERY_CONFIG = FwQueryConfig(
|
||||
FW_VERSIONS = {
|
||||
CAR.ESCAPE_MK4: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-NY\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-SA\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'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ6T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x7E0, None): [
|
||||
b'LX6A-14C204-BJV\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'MX6A-14C204-BEF\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.shiftByWire, 0x732, None): [
|
||||
b'LX6P-14G395-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6P-14G395-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
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',
|
||||
b'M1MC-14D003-AB\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',
|
||||
b'L1MC-2D053-KB\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',
|
||||
@@ -123,10 +133,12 @@ FW_VERSIONS = {
|
||||
(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',
|
||||
b'NB5A-14C204-HB\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',
|
||||
b'L1MP-14G395-JB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FOCUS_MK4: {
|
||||
|
||||
@@ -5,10 +5,11 @@ from common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.gm import gmcan
|
||||
from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, EV_CAR
|
||||
from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
NetworkLocation = car.CarParams.NetworkLocation
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
# Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s
|
||||
CAMERA_CANCEL_DELAY_FRAMES = 10
|
||||
@@ -30,7 +31,7 @@ class CarController:
|
||||
self.sent_lka_steering_cmd = False
|
||||
self.lka_icon_status_last = (False, False)
|
||||
|
||||
self.params = CarControllerParams()
|
||||
self.params = CarControllerParams(self.CP)
|
||||
|
||||
self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt'])
|
||||
self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar'])
|
||||
@@ -83,20 +84,23 @@ class CarController:
|
||||
self.apply_gas = self.params.INACTIVE_REGEN
|
||||
self.apply_brake = 0
|
||||
else:
|
||||
if self.CP.carFingerprint in EV_CAR:
|
||||
self.apply_gas = int(round(interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
|
||||
self.apply_brake = int(round(interp(actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
|
||||
else:
|
||||
self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
|
||||
self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
|
||||
self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V)))
|
||||
self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
|
||||
|
||||
idx = (self.frame // 4) % 4
|
||||
|
||||
at_full_stop = CC.longActive and CS.out.standstill
|
||||
near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE)
|
||||
friction_brake_bus = CanBus.CHASSIS
|
||||
# GM Camera exceptions
|
||||
# TODO: can we always check the longControlState?
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
at_full_stop = at_full_stop and actuators.longControlState == LongCtrlState.stopping
|
||||
friction_brake_bus = CanBus.POWERTRAIN
|
||||
|
||||
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
|
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop))
|
||||
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop))
|
||||
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake, idx, CC.enabled, near_stop, at_full_stop, self.CP))
|
||||
|
||||
# Send dashboard UI commands (ACC status)
|
||||
send_fcw = hud_alert == VisualAlert.fcw
|
||||
|
||||
@@ -28,6 +28,7 @@ class CarState(CarStateBase):
|
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
||||
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
|
||||
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
|
||||
self.moving_backward = pt_cp.vl["EBCMWheelSpdRear"]["MovingBackward"] != 0
|
||||
|
||||
# Variables used for avoiding LKAS faults
|
||||
self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0
|
||||
@@ -50,16 +51,19 @@ class CarState(CarStateBase):
|
||||
else:
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None))
|
||||
|
||||
# Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
|
||||
# that the brake is being intermittently pressed without user interaction.
|
||||
# To avoid a cruise fault we need to match the ECM's brake pressed signal and threshold
|
||||
# https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf
|
||||
ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"]
|
||||
ret.brakePressed = ret.brake >= 8
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["BrakePressed"] != 0
|
||||
else:
|
||||
# Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
|
||||
# that the brake is being intermittently pressed without user interaction.
|
||||
# To avoid a cruise fault we need to use a conservative brake position threshold
|
||||
# https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf
|
||||
ret.brakePressed = ret.brake >= 8
|
||||
|
||||
# Regen braking is braking
|
||||
if self.CP.transmissionType == TransmissionType.direct:
|
||||
ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
|
||||
ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
|
||||
|
||||
ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254.
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
@@ -96,6 +100,9 @@ class CarState(CarStateBase):
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS
|
||||
ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
|
||||
# openpilot controls nonAdaptive when not pcmCruise
|
||||
if self.CP.pcmCruise:
|
||||
ret.cruiseState.nonAdaptive = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCruiseState"] not in (2, 3)
|
||||
|
||||
return ret
|
||||
|
||||
@@ -108,6 +115,7 @@ class CarState(CarStateBase):
|
||||
("AEBCmdActive", "AEBCmd"),
|
||||
("RollingCounter", "ASCMLKASteeringCmd"),
|
||||
("ACCSpeedSetpoint", "ASCMActiveCruiseControlStatus"),
|
||||
("ACCCruiseState", "ASCMActiveCruiseControlStatus"),
|
||||
]
|
||||
checks += [
|
||||
("AEBCmd", 10),
|
||||
@@ -139,6 +147,7 @@ class CarState(CarStateBase):
|
||||
("FRWheelSpd", "EBCMWheelSpdFront"),
|
||||
("RLWheelSpd", "EBCMWheelSpdRear"),
|
||||
("RRWheelSpd", "EBCMWheelSpdRear"),
|
||||
("MovingBackward", "EBCMWheelSpdRear"),
|
||||
("PRNDL2", "ECMPRDNL2"),
|
||||
("ManualMode", "ECMPRDNL2"),
|
||||
("LKADriverAppldTrq", "PSCMStatus"),
|
||||
@@ -152,6 +161,7 @@ class CarState(CarStateBase):
|
||||
("TractionControlOn", "ESPStatus"),
|
||||
("ParkBrake", "VehicleIgnitionAlt"),
|
||||
("CruiseMainOn", "ECMEngineStatus"),
|
||||
("BrakePressed", "ECMEngineStatus"),
|
||||
]
|
||||
|
||||
checks = [
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
from selfdrive.car import make_can_msg
|
||||
from selfdrive.car.gm.values import CAR
|
||||
|
||||
|
||||
def create_buttons(packer, bus, idx, button):
|
||||
@@ -52,15 +53,20 @@ def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop):
|
||||
return packer.make_can_msg("ASCMGasRegenCmd", bus, values)
|
||||
|
||||
|
||||
def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_full_stop):
|
||||
def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_stop, at_full_stop, CP):
|
||||
mode = 0x1
|
||||
|
||||
# TODO: Understand this better. Volts and ICE Camera ACC cars are 0x1 when enabled with no brake
|
||||
if enabled and CP.carFingerprint in (CAR.BOLT_EUV,):
|
||||
mode = 0x9
|
||||
|
||||
if apply_brake > 0:
|
||||
mode = 0xa
|
||||
if at_full_stop:
|
||||
mode = 0xd
|
||||
|
||||
# TODO: this is to have GM bringing the car to complete stop,
|
||||
# but currently it conflicts with OP controls, so turned off.
|
||||
# but currently it conflicts with OP controls, so turned off. Not set by all cars
|
||||
#elif near_stop:
|
||||
# mode = 0xb
|
||||
|
||||
|
||||
@@ -20,8 +20,7 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, current_speed, cruise_speed):
|
||||
params = CarControllerParams()
|
||||
return params.ACCEL_MIN, params.ACCEL_MAX
|
||||
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
|
||||
|
||||
# Determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
|
||||
@staticmethod
|
||||
@@ -56,13 +55,34 @@ class CarInterface(CarInterfaceBase):
|
||||
else:
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
|
||||
ret.longitudinalTuning.deadzoneBP = [0.]
|
||||
ret.longitudinalTuning.deadzoneV = [0.15]
|
||||
|
||||
ret.longitudinalTuning.kpBP = [5., 35.]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
|
||||
if candidate in CAMERA_ACC_CAR:
|
||||
ret.openpilotLongitudinalControl = False
|
||||
ret.experimentalLongitudinalAvailable = True
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
ret.radarOffCan = True # no radar
|
||||
ret.pcmCruise = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM
|
||||
ret.minEnableSpeed = 5 * CV.KPH_TO_MS
|
||||
|
||||
if experimental_long:
|
||||
ret.pcmCruise = False
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
|
||||
|
||||
# Tuning
|
||||
ret.longitudinalTuning.kpV = [2.0, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.72]
|
||||
ret.stopAccel = -2.0
|
||||
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
|
||||
ret.vEgoStopping = 0.25
|
||||
ret.vEgoStarting = 0.25
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.5
|
||||
|
||||
else: # ASCM, OBD-II harness
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.networkLocation = NetworkLocation.gateway
|
||||
@@ -71,6 +91,10 @@ class CarInterface(CarInterfaceBase):
|
||||
# supports stop and go, but initial engage must (conservatively) be above 18mph
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
|
||||
# Tuning
|
||||
ret.longitudinalTuning.kpV = [2.4, 1.5]
|
||||
ret.longitudinalTuning.kiV = [0.36]
|
||||
|
||||
CarInterfaceBase.configure_dp_tune(candidate, ret.lateralTuning)
|
||||
|
||||
# These cars have been put into dashcam only due to both a lack of users and test coverage.
|
||||
@@ -87,11 +111,6 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
|
||||
ret.longitudinalTuning.kpBP = [5., 35.]
|
||||
ret.longitudinalTuning.kpV = [2.4, 1.5]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.36]
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz
|
||||
|
||||
@@ -195,23 +214,27 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
|
||||
|
||||
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
|
||||
be = create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)
|
||||
buttonEvents = [create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)]
|
||||
# Handle ACCButtons changing buttons mid-press
|
||||
if self.CS.cruise_buttons != CruiseButtons.UNPRESS and self.CS.prev_cruise_buttons != CruiseButtons.UNPRESS:
|
||||
buttonEvents.append(create_button_event(CruiseButtons.UNPRESS, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS))
|
||||
|
||||
# Suppress resume button if we're resuming from stop so we don't adjust speed.
|
||||
if be.type == ButtonType.accelCruise and (ret.cruiseState.enabled and ret.standstill):
|
||||
be.type = ButtonType.unknown
|
||||
|
||||
ret.buttonEvents = [be]
|
||||
ret.buttonEvents = buttonEvents
|
||||
|
||||
# The ECM allows enabling on falling edge of set, but only rising edge of resume
|
||||
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low,
|
||||
GearShifter.eco, GearShifter.manumatic],
|
||||
pcm_enable=self.CP.pcmCruise)
|
||||
pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,))
|
||||
events = self.dp_atl_warning(ret, events)
|
||||
if not self.CP.pcmCruise:
|
||||
if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents):
|
||||
events.add(EventName.buttonEnable)
|
||||
|
||||
# Enabling at a standstill with brake is allowed
|
||||
# TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs
|
||||
if ret.vEgo < self.CP.minEnableSpeed and not (ret.standstill and ret.brake >= 20 and
|
||||
self.CP.networkLocation == NetworkLocation.fwdCamera):
|
||||
below_min_enable_speed = ret.vEgo < self.CP.minEnableSpeed or self.CS.moving_backward
|
||||
if below_min_enable_speed and not (ret.standstill and ret.brake >= 20 and
|
||||
self.CP.networkLocation == NetworkLocation.fwdCamera):
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
if ret.cruiseState.standstill:
|
||||
events.add(EventName.resumeRequired)
|
||||
|
||||
+39
-26
@@ -1,5 +1,5 @@
|
||||
from collections import defaultdict
|
||||
from dataclasses import dataclass, field
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
from typing import Dict, List, Union
|
||||
|
||||
@@ -24,15 +24,6 @@ class CarControllerParams:
|
||||
ADAS_KEEPALIVE_STEP = 100
|
||||
CAMERA_KEEPALIVE_STEP = 100
|
||||
|
||||
# Volt gas/brake lookups
|
||||
# TODO: These values should be confirmed on non-Volt vehicles.
|
||||
# MAX_GAS should achieve 2 m/s^2 and MAX_BRAKE with regen should achieve -4.0 m/s^2
|
||||
MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill.
|
||||
ZERO_GAS = 2048 # Coasting
|
||||
MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen
|
||||
MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen
|
||||
INACTIVE_REGEN = 1404
|
||||
|
||||
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
|
||||
# perform the closed loop control, and might need some
|
||||
# to apply some more braking if we're on a downhill slope.
|
||||
@@ -41,15 +32,32 @@ class CarControllerParams:
|
||||
ACCEL_MAX = 2. # m/s^2
|
||||
ACCEL_MIN = -4. # m/s^2
|
||||
|
||||
# ICE has much less engine braking force compared to regen in EVs,
|
||||
# lower threshold removes some braking deadzone
|
||||
GAS_LOOKUP_BP = [-0.1, 0., ACCEL_MAX]
|
||||
EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX]
|
||||
GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
|
||||
def __init__(self, CP):
|
||||
# Gas/brake lookups
|
||||
self.ZERO_GAS = 2048 # Coasting
|
||||
self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen
|
||||
|
||||
BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1]
|
||||
EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.]
|
||||
BRAKE_LOOKUP_V = [MAX_BRAKE, 0.]
|
||||
if CP.carFingerprint in CAMERA_ACC_CAR:
|
||||
self.MAX_GAS = 3400
|
||||
self.MAX_ACC_REGEN = 1514
|
||||
self.INACTIVE_REGEN = 1554
|
||||
# Camera ACC vehicles have no regen while enabled.
|
||||
# Camera transitions to MAX_ACC_REGEN from ZERO_GAS and uses friction brakes instantly
|
||||
max_regen_acceleration = 0.
|
||||
|
||||
else:
|
||||
self.MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill.
|
||||
self.MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen
|
||||
self.INACTIVE_REGEN = 1404
|
||||
# ICE has much less engine braking force compared to regen in EVs,
|
||||
# lower threshold removes some braking deadzone
|
||||
max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1
|
||||
|
||||
self.GAS_LOOKUP_BP = [max_regen_acceleration, 0., self.ACCEL_MAX]
|
||||
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS]
|
||||
|
||||
self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, max_regen_acceleration]
|
||||
self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.]
|
||||
|
||||
|
||||
class CAR:
|
||||
@@ -76,25 +84,30 @@ class Footnote(Enum):
|
||||
@dataclass
|
||||
class GMCarInfo(CarInfo):
|
||||
package: str = "Adaptive Cruise Control (ACC)"
|
||||
harness: Enum = Harness.obd_ii
|
||||
footnotes: List[Enum] = field(default_factory=lambda: [Footnote.OBD_II])
|
||||
|
||||
def init_make(self, CP: car.CarParams):
|
||||
if CP.networkLocation == car.CarParams.NetworkLocation.fwdCamera:
|
||||
self.harness = Harness.gm
|
||||
else:
|
||||
self.harness = Harness.obd_ii
|
||||
self.footnotes.append(Footnote.OBD_II)
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = {
|
||||
CAR.HOLDEN_ASTRA: GMCarInfo("Holden Astra 2017"),
|
||||
CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", min_enable_speed=0),
|
||||
CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", min_enable_speed=0, video_link="https://youtu.be/QeMCN_4TFfQ"),
|
||||
CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"),
|
||||
CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017"),
|
||||
CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"),
|
||||
CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"),
|
||||
CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"),
|
||||
CAR.BOLT_EV: GMCarInfo("Chevrolet Bolt EV 2022-23", footnotes=[], harness=Harness.gm),
|
||||
CAR.BOLT_EUV: GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", "https://youtu.be/xvwzGMUA210", footnotes=[], harness=Harness.gm),
|
||||
CAR.BOLT_EV: GMCarInfo("Chevrolet Bolt EV 2022-23"),
|
||||
CAR.BOLT_EUV: GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", "https://youtu.be/xvwzGMUA210"),
|
||||
CAR.SILVERADO: [
|
||||
GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II", footnotes=[], harness=Harness.gm),
|
||||
GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", footnotes=[], harness=Harness.gm),
|
||||
GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II"),
|
||||
GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", "https://youtu.be/5HbNoBLzRwE"),
|
||||
],
|
||||
CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22", footnotes=[], harness=Harness.gm),
|
||||
CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22"),
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -99,6 +99,12 @@ HUDData = namedtuple("HUDData",
|
||||
"lanes_visible", "fcw", "acc_alert", "steer_required"])
|
||||
|
||||
|
||||
def rate_limit_steer(new_steer, last_steer):
|
||||
# TODO just hardcoded ramp to min/max in 0.33s for all Honda
|
||||
MAX_DELTA = 3 * DT_CTRL
|
||||
return clip(new_steer, last_steer - MAX_DELTA, last_steer + MAX_DELTA)
|
||||
|
||||
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
@@ -116,6 +122,7 @@ class CarController:
|
||||
self.speed = 0.0
|
||||
self.gas = 0.0
|
||||
self.brake = 0.0
|
||||
self.last_steer = 0.0
|
||||
|
||||
def update(self, CC, CS, dragonconf):
|
||||
actuators = CC.actuators
|
||||
@@ -130,6 +137,10 @@ class CarController:
|
||||
accel = 0.0
|
||||
gas, brake = 0.0, 0.0
|
||||
|
||||
# *** rate limit steer ***
|
||||
limited_steer = rate_limit_steer(actuators.steer, self.last_steer)
|
||||
self.last_steer = limited_steer
|
||||
|
||||
# *** apply brake hysteresis ***
|
||||
pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady,
|
||||
CS.out.vEgo, self.CP.carFingerprint)
|
||||
@@ -143,7 +154,7 @@ class CarController:
|
||||
# **** process the car messages ****
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
apply_steer = int(interp(-actuators.steer * self.params.STEER_MAX,
|
||||
apply_steer = int(interp(-limited_steer * self.params.STEER_MAX,
|
||||
self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
|
||||
|
||||
# Send CAN commands
|
||||
@@ -198,11 +209,6 @@ class CarController:
|
||||
|
||||
else:
|
||||
# Send gas and brake commands.
|
||||
if not CS.out.cruiseActualEnabled:
|
||||
accel = 0.
|
||||
brake = 0.
|
||||
self.brake_last = 0.
|
||||
|
||||
if self.frame % 2 == 0:
|
||||
ts = self.frame * DT_CTRL
|
||||
|
||||
@@ -255,6 +261,7 @@ class CarController:
|
||||
new_actuators.accel = self.accel
|
||||
new_actuators.gas = self.gas
|
||||
new_actuators.brake = self.brake
|
||||
new_actuators.steer = self.last_steer
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -42,6 +42,8 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
|
||||
("CRUISE_SETTING", "SCM_BUTTONS"),
|
||||
("ACC_STATUS", "POWERTRAIN_DATA"),
|
||||
("MAIN_ON", main_on_sig_msg),
|
||||
#dp
|
||||
("ENGINE_RPM", "POWERTRAIN_DATA"),
|
||||
]
|
||||
|
||||
checks = [
|
||||
@@ -89,6 +91,8 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
|
||||
("CRUISE_SPEED", "ACC_HUD"),
|
||||
("ACCEL_COMMAND", "ACC_CONTROL"),
|
||||
("AEB_STATUS", "ACC_CONTROL"),
|
||||
#dp
|
||||
("BRAKE_LIGHTS", "ACC_CONTROL"),
|
||||
]
|
||||
checks += [
|
||||
("ACC_HUD", 10),
|
||||
@@ -150,6 +154,7 @@ class CarState(CarStateBase):
|
||||
self.brake_switch_active = False
|
||||
self.cruise_setting = 0
|
||||
self.v_cruise_pcm_prev = 0
|
||||
self.engineRPM = 0
|
||||
|
||||
# When available we use cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] to populate vEgoCluster
|
||||
# However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX)
|
||||
@@ -219,6 +224,8 @@ class CarState(CarStateBase):
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
|
||||
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
|
||||
ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
|
||||
#dp
|
||||
self.engineRPM = cp.vl["POWERTRAIN_DATA"]['ENGINE_RPM']
|
||||
|
||||
# TODO: set for all cars
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}):
|
||||
@@ -278,6 +285,17 @@ class CarState(CarStateBase):
|
||||
if ret.brake > 0.1:
|
||||
ret.brakePressed = True
|
||||
|
||||
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
|
||||
CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
|
||||
ret.brakeLightsDEPRECATED = bool(ret.brakePressed or cp.vl["ACC_CONTROL"]['BRAKE_LIGHTS'] != 0 or ret.brake > 0.4 or ret.parkingBrake) if not self.CP.openpilotLongitudinalControl else \
|
||||
bool(ret.brakePressed or ret.brake > 0.4 or ret.parkingBrake)
|
||||
elif self.CP.carFingerprint in HONDA_BOSCH and self.CP.carFingerprint not in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
|
||||
CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E) and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
|
||||
ret.brakeLightsDEPRECATED = bool(ret.brakePressed or cp.vl["ACC_CONTROL"]['BRAKE_LIGHTS'] != 0 or ret.brake > 0.4) if not self.CP.openpilotLongitudinalControl else \
|
||||
bool(ret.brakePressed or ret.brake > 0.4)
|
||||
else:
|
||||
ret.brakeLightsDEPRECATED = bool(ret.brakePressed)
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
# TODO: find the radarless AEB_STATUS bit and make sure ACCEL_COMMAND is correct to enable AEB alerts
|
||||
if self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
|
||||
|
||||
@@ -52,6 +52,13 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
ret.pcmCruise = not ret.enableGasInterceptor
|
||||
|
||||
# dp - attempt to disable op long
|
||||
params = Params()
|
||||
if int(params.get("dp_atl").decode('utf-8')) == 1:
|
||||
ret.openpilotLongitudinalControl = False
|
||||
if candidate in HONDA_BOSCH:
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
|
||||
if candidate == CAR.CRV_5G:
|
||||
ret.enableBsm = 0x12f8bfa7 in fingerprint[0]
|
||||
|
||||
@@ -221,23 +228,17 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
|
||||
tire_stiffness_factor = 0.677
|
||||
|
||||
elif candidate == CAR.ODYSSEY:
|
||||
ret.mass = 4471. * CV.LB_TO_KG + STD_CARGO_KG
|
||||
elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN):
|
||||
ret.mass = 1900. + STD_CARGO_KG
|
||||
ret.wheelbase = 3.00
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 14.35 # as spec
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
tire_stiffness_factor = 0.82
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
|
||||
|
||||
elif candidate == CAR.ODYSSEY_CHN:
|
||||
ret.mass = 1849.2 + STD_CARGO_KG # mean of 4 models in kg
|
||||
ret.wheelbase = 2.90
|
||||
ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY
|
||||
ret.steerRatio = 14.35
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
|
||||
tire_stiffness_factor = 0.82
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
|
||||
if candidate == CAR.ODYSSEY_CHN:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
|
||||
else:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
|
||||
elif candidate in (CAR.PILOT, CAR.PASSPORT):
|
||||
ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight
|
||||
@@ -278,10 +279,6 @@ class CarInterface(CarInterfaceBase):
|
||||
else:
|
||||
raise ValueError(f"unsupported car {candidate}")
|
||||
|
||||
params = Params()
|
||||
if int(params.get("dp_atl").decode('utf-8')) == 1:
|
||||
ret.openpilotLongitudinalControl = False
|
||||
|
||||
# These cars use alternate user brake msg (0x1BE)
|
||||
if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE
|
||||
@@ -345,6 +342,9 @@ class CarInterface(CarInterfaceBase):
|
||||
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
|
||||
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
|
||||
|
||||
#dp
|
||||
ret.engineRPM = self.CS.engineRPM
|
||||
|
||||
buttonEvents = []
|
||||
|
||||
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
|
||||
|
||||
@@ -106,40 +106,46 @@ class Footnote(Enum):
|
||||
class HondaCarInfo(CarInfo):
|
||||
package: str = "Honda Sensing"
|
||||
|
||||
def init_make(self, CP: car.CarParams):
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
self.harness = Harness.bosch_b if CP.carFingerprint in HONDA_BOSCH_RADARLESS else Harness.bosch_a
|
||||
else:
|
||||
self.harness = Harness.nidec
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = {
|
||||
CAR.ACCORD: [
|
||||
HondaCarInfo("Honda Accord 2018-22", "All", "https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a),
|
||||
HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a),
|
||||
HondaCarInfo("Honda Accord 2018-22", "All", "https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
],
|
||||
CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a),
|
||||
CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec, video_link="https://youtu.be/-IkImTe1NYE"),
|
||||
CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE"),
|
||||
CAR.CIVIC_BOSCH: [
|
||||
HondaCarInfo("Honda Civic 2019-21", "All", "https://www.youtube.com/watch?v=4Iz1Mz5LGF8", [Footnote.CIVIC_DIESEL], 2. * CV.MPH_TO_MS, harness=Harness.bosch_a),
|
||||
HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.bosch_a),
|
||||
HondaCarInfo("Honda Civic 2019-21", "All", "https://www.youtube.com/watch?v=4Iz1Mz5LGF8", [Footnote.CIVIC_DIESEL], 2. * CV.MPH_TO_MS),
|
||||
HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
],
|
||||
CAR.CIVIC_BOSCH_DIESEL: None, # same platform
|
||||
CAR.CIVIC_2022: [
|
||||
HondaCarInfo("Honda Civic 2022", "All", harness=Harness.bosch_b),
|
||||
HondaCarInfo("Honda Civic Hatchback 2022", "All", harness=Harness.bosch_b),
|
||||
HondaCarInfo("Honda Civic 2022", "All"),
|
||||
HondaCarInfo("Honda Civic Hatchback 2022", "All"),
|
||||
],
|
||||
CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS, harness=Harness.nidec),
|
||||
CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec),
|
||||
CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.bosch_a),
|
||||
CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS),
|
||||
CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.CRV_EU: None, # HondaCarInfo("Honda CR-V EU", "Touring"), # Euro version of CRV Touring
|
||||
CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.bosch_a),
|
||||
CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec),
|
||||
CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec),
|
||||
CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec),
|
||||
CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20", harness=Harness.nidec),
|
||||
CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20"),
|
||||
CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey
|
||||
CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec),
|
||||
CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a),
|
||||
CAR.PILOT: HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec),
|
||||
CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec),
|
||||
CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", min_steer_speed=12. * CV.MPH_TO_MS, harness=Harness.nidec),
|
||||
CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a),
|
||||
CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch_a),
|
||||
CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
CAR.PILOT: HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
}
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
@@ -1032,6 +1038,23 @@ FW_VERSIONS = {
|
||||
b'54008-THR-A020\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.ODYSSEY_CHN: {
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-T6D-H220\x00\x00',
|
||||
],
|
||||
(Ecu.gateway, 0x18daeff1, None): [
|
||||
b'38897-T6A-J010\x00\x00',
|
||||
],
|
||||
(Ecu.combinationMeter, 0x18da60f1, None): [
|
||||
b'78109-T6A-F310\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x18dab0f1, None): [
|
||||
b'36161-T6A-P040\x00\x00',
|
||||
],
|
||||
(Ecu.srs, 0x18da53f1, None): [
|
||||
b'77959-T6A-P110\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.PILOT: {
|
||||
(Ecu.shiftByWire, 0x18da0bf1, None): [
|
||||
b'54008-TG7-A520\x00\x00',
|
||||
|
||||
@@ -49,6 +49,7 @@ class CarController:
|
||||
self.angle_limit_counter = 0
|
||||
self.frame = 0
|
||||
|
||||
self.accel_last = 0
|
||||
self.apply_steer_last = 0
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.last_button_frame = 0
|
||||
@@ -84,7 +85,7 @@ class CarController:
|
||||
# *** common hyundai stuff ***
|
||||
|
||||
# tester present - w/ no response (keeps relevant ECU disabled)
|
||||
if self.frame % 100 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl:
|
||||
addr, bus = 0x7d0, 0
|
||||
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
|
||||
addr, bus = 0x730, 5
|
||||
@@ -121,10 +122,12 @@ class CarController:
|
||||
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled))
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame))
|
||||
if hda2:
|
||||
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame))
|
||||
if self.frame % 2 == 0:
|
||||
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, accel, stopping, CC.cruiseControl.override,
|
||||
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CP, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
|
||||
set_speed_in_units))
|
||||
self.accel_last = accel
|
||||
else:
|
||||
# button presses
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
|
||||
@@ -173,7 +176,7 @@ class CarController:
|
||||
if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
|
||||
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV, CAR.KONA_EV_2022,
|
||||
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
|
||||
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
|
||||
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022):
|
||||
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
|
||||
|
||||
# 5 Hz ACC options
|
||||
|
||||
@@ -21,8 +21,9 @@ class CarState(CarStateBase):
|
||||
self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
|
||||
self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
|
||||
|
||||
self.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else "GEAR_SHIFTER"
|
||||
if CP.carFingerprint in CANFD_CAR:
|
||||
self.shifter_values = can_define.dv["GEAR_SHIFTER"]["GEAR"]
|
||||
self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"]
|
||||
elif self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
|
||||
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
|
||||
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
|
||||
@@ -84,6 +85,8 @@ class CarState(CarStateBase):
|
||||
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
|
||||
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
|
||||
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
|
||||
#dp
|
||||
ret.engineRPM = cp.vl["TCU_DCT13"]['Cluster_Engine_RPM']
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD
|
||||
ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
|
||||
|
||||
@@ -104,6 +107,9 @@ class CarState(CarStateBase):
|
||||
ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
|
||||
ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
|
||||
ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
|
||||
#dp
|
||||
ret.brakeLightsDEPRECATED = bool(cp.vl["TCS13"]["BrakeLight"] or ret.brakePressed or ret.brakeHoldActive or ret.parkingBrake)
|
||||
|
||||
|
||||
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
|
||||
if self.CP.carFingerprint in HYBRID_CAR:
|
||||
@@ -154,17 +160,21 @@ class CarState(CarStateBase):
|
||||
def update_canfd(self, cp, cp_cam):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
if self.CP.carFingerprint in EV_CAR:
|
||||
ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255.
|
||||
elif self.CP.carFingerprint in HYBRID_CAR:
|
||||
ret.gas = cp.vl["ACCELERATOR_ALT"]["ACCELERATOR_PEDAL"] / 1023.
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
if self.CP.carFingerprint in (EV_CAR | HYBRID_CAR):
|
||||
if self.CP.carFingerprint in EV_CAR:
|
||||
ret.gas = cp.vl["ACCELERATOR"]["ACCELERATOR_PEDAL"] / 255.
|
||||
else:
|
||||
ret.gas = cp.vl["ACCELERATOR_ALT"]["ACCELERATOR_PEDAL"] / 1023.
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
else:
|
||||
ret.gasPressed = bool(cp.vl["ACCELERATOR_BRAKE_ALT"]["ACCELERATOR_PEDAL_PRESSED"])
|
||||
|
||||
ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1
|
||||
|
||||
ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR_OPEN"] == 1
|
||||
ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT_LATCHED"] == 0
|
||||
|
||||
gear = cp.vl["GEAR_SHIFTER"]["GEAR"]
|
||||
gear = cp.vl[self.gear_msg_canfd]["GEAR"]
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
|
||||
|
||||
# TODO: figure out positions
|
||||
@@ -195,11 +205,11 @@ class CarState(CarStateBase):
|
||||
self.is_metric = cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] != 1
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
|
||||
cp_cruise_info = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
|
||||
ret.cruiseState.speed = cp_cruise_info.vl["CRUISE_INFO"]["SET_SPEED"] * speed_factor
|
||||
ret.cruiseState.standstill = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STANDSTILL"] == 1
|
||||
ret.cruiseState.enabled = cp_cruise_info.vl["CRUISE_INFO"]["CRUISE_STATUS"] != 0
|
||||
self.cruise_info = copy.copy(cp_cruise_info.vl["CRUISE_INFO"])
|
||||
cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
|
||||
ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
|
||||
ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1
|
||||
ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2)
|
||||
self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"])
|
||||
|
||||
cruise_btn_msg = "CRUISE_BUTTONS_ALT" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS"
|
||||
self.prev_cruise_buttons = self.cruise_buttons[-1]
|
||||
@@ -272,6 +282,9 @@ class CarState(CarStateBase):
|
||||
|
||||
("SAS_Angle", "SAS11"),
|
||||
("SAS_Speed", "SAS11"),
|
||||
#dp
|
||||
("Cluster_Engine_RPM", "TCU_DCT13"),
|
||||
("BrakeLight", "TCS13"),
|
||||
]
|
||||
checks = [
|
||||
# address, frequency
|
||||
@@ -286,6 +299,7 @@ class CarState(CarStateBase):
|
||||
("CGW4", 5),
|
||||
("WHL_SPD11", 50),
|
||||
("SAS11", 100),
|
||||
("TCU_DCT13", 100),
|
||||
]
|
||||
|
||||
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in CAMERA_SCC_CAR:
|
||||
@@ -412,13 +426,14 @@ class CarState(CarStateBase):
|
||||
def get_can_parser_canfd(CP):
|
||||
|
||||
cruise_btn_msg = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "CRUISE_BUTTONS"
|
||||
gear_msg = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else "GEAR_SHIFTER"
|
||||
signals = [
|
||||
("WHEEL_SPEED_1", "WHEEL_SPEEDS"),
|
||||
("WHEEL_SPEED_2", "WHEEL_SPEEDS"),
|
||||
("WHEEL_SPEED_3", "WHEEL_SPEEDS"),
|
||||
("WHEEL_SPEED_4", "WHEEL_SPEEDS"),
|
||||
|
||||
("GEAR", "GEAR_SHIFTER"),
|
||||
("GEAR", gear_msg),
|
||||
|
||||
("STEERING_RATE", "STEERING_SENSORS"),
|
||||
("STEERING_ANGLE", "STEERING_SENSORS"),
|
||||
@@ -443,8 +458,7 @@ class CarState(CarStateBase):
|
||||
|
||||
checks = [
|
||||
("WHEEL_SPEEDS", 100),
|
||||
("GEAR_SHIFTER", 100),
|
||||
("BRAKE", 100),
|
||||
(gear_msg, 100),
|
||||
("STEERING_SENSORS", 100),
|
||||
("MDPS", 100),
|
||||
("TCS", 50),
|
||||
@@ -463,14 +477,14 @@ class CarState(CarStateBase):
|
||||
("BLINDSPOTS_REAR_CORNERS", 20),
|
||||
]
|
||||
|
||||
if CP.flags & HyundaiFlags.CANFD_HDA2 and not CP.openpilotLongitudinalControl:
|
||||
if not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and not CP.openpilotLongitudinalControl:
|
||||
signals += [
|
||||
("CRUISE_STATUS", "CRUISE_INFO"),
|
||||
("SET_SPEED", "CRUISE_INFO"),
|
||||
("CRUISE_STANDSTILL", "CRUISE_INFO"),
|
||||
("ACCMode", "SCC_CONTROL"),
|
||||
("VSetDis", "SCC_CONTROL"),
|
||||
("CRUISE_STANDSTILL", "SCC_CONTROL"),
|
||||
]
|
||||
checks += [
|
||||
("CRUISE_INFO", 50),
|
||||
("SCC_CONTROL", 50),
|
||||
]
|
||||
|
||||
if CP.carFingerprint in EV_CAR:
|
||||
@@ -487,32 +501,40 @@ class CarState(CarStateBase):
|
||||
checks += [
|
||||
("ACCELERATOR_ALT", 100),
|
||||
]
|
||||
else:
|
||||
signals += [
|
||||
("ACCELERATOR_PEDAL_PRESSED", "ACCELERATOR_BRAKE_ALT"),
|
||||
]
|
||||
checks += [
|
||||
("ACCELERATOR_BRAKE_ALT", 100),
|
||||
]
|
||||
|
||||
bus = 5 if CP.flags & HyundaiFlags.CANFD_HDA2 else 4
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, bus)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser_canfd(CP):
|
||||
signals = []
|
||||
checks = []
|
||||
if CP.flags & HyundaiFlags.CANFD_HDA2:
|
||||
signals = [(f"BYTE{i}", "CAM_0x2a4") for i in range(3, 24)]
|
||||
checks = [("CAM_0x2a4", 20)]
|
||||
else:
|
||||
signals = [
|
||||
("COUNTER", "CRUISE_INFO"),
|
||||
("NEW_SIGNAL_1", "CRUISE_INFO"),
|
||||
("CRUISE_MAIN", "CRUISE_INFO"),
|
||||
("CRUISE_STATUS", "CRUISE_INFO"),
|
||||
("CRUISE_INACTIVE", "CRUISE_INFO"),
|
||||
("ZEROS_9", "CRUISE_INFO"),
|
||||
("CRUISE_STANDSTILL", "CRUISE_INFO"),
|
||||
("ZEROS_5", "CRUISE_INFO"),
|
||||
("DISTANCE_SETTING", "CRUISE_INFO"),
|
||||
("SET_SPEED", "CRUISE_INFO"),
|
||||
("NEW_SIGNAL_4", "CRUISE_INFO"),
|
||||
signals += [(f"BYTE{i}", "CAM_0x2a4") for i in range(3, 24)]
|
||||
checks += [("CAM_0x2a4", 20)]
|
||||
elif CP.flags & HyundaiFlags.CANFD_CAMERA_SCC:
|
||||
signals += [
|
||||
("COUNTER", "SCC_CONTROL"),
|
||||
("NEW_SIGNAL_1", "SCC_CONTROL"),
|
||||
("MainMode_ACC", "SCC_CONTROL"),
|
||||
("ACCMode", "SCC_CONTROL"),
|
||||
("CRUISE_INACTIVE", "SCC_CONTROL"),
|
||||
("ZEROS_9", "SCC_CONTROL"),
|
||||
("CRUISE_STANDSTILL", "SCC_CONTROL"),
|
||||
("ZEROS_5", "SCC_CONTROL"),
|
||||
("DISTANCE_SETTING", "SCC_CONTROL"),
|
||||
("VSetDis", "SCC_CONTROL"),
|
||||
]
|
||||
|
||||
checks = [
|
||||
("CRUISE_INFO", 50),
|
||||
checks += [
|
||||
("SCC_CONTROL", 50),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 6)
|
||||
|
||||
@@ -20,7 +20,8 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
|
||||
if car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE,
|
||||
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020,
|
||||
CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022,
|
||||
CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022):
|
||||
CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022,
|
||||
CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022):
|
||||
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
|
||||
values["CF_Lkas_LdwsOpt_USM"] = 2
|
||||
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
from common.numpy_fast import clip
|
||||
from selfdrive.car.hyundai.values import HyundaiFlags
|
||||
|
||||
|
||||
@@ -52,10 +53,9 @@ def create_buttons(packer, CP, cnt, btn):
|
||||
def create_acc_cancel(packer, CP, cruise_info_copy):
|
||||
values = cruise_info_copy
|
||||
values.update({
|
||||
"CRUISE_STATUS": 0,
|
||||
"CRUISE_INACTIVE": 1,
|
||||
"ACCMode": 4,
|
||||
})
|
||||
return packer.make_can_msg("CRUISE_INFO", get_e_can_bus(CP), values)
|
||||
return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values)
|
||||
|
||||
def create_lfahda_cluster(packer, CP, enabled):
|
||||
values = {
|
||||
@@ -65,33 +65,37 @@ def create_lfahda_cluster(packer, CP, enabled):
|
||||
return packer.make_can_msg("LFAHDA_CLUSTER", get_e_can_bus(CP), values)
|
||||
|
||||
|
||||
def create_acc_control(packer, CP, enabled, accel, stopping, gas_override, set_speed):
|
||||
cruise_status = 0 if not enabled else (4 if gas_override else 2)
|
||||
def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_override, set_speed):
|
||||
jerk = 5
|
||||
jn = jerk / 50
|
||||
if not enabled or gas_override:
|
||||
accel = 0
|
||||
a_val, a_raw = 0, 0
|
||||
else:
|
||||
a_raw = accel
|
||||
a_val = clip(accel, accel_last - jn, accel_last + jn)
|
||||
if stopping:
|
||||
a_raw = 0
|
||||
|
||||
values = {
|
||||
"CRUISE_STATUS": cruise_status,
|
||||
"CRUISE_INACTIVE": 0 if enabled else 1,
|
||||
"CRUISE_MAIN": 1,
|
||||
"CRUISE_STANDSTILL": 0,
|
||||
"STOP_REQ": 1 if stopping else 0,
|
||||
"ACCEL_REQ": accel,
|
||||
"ACCEL_REQ2": accel,
|
||||
"SET_SPEED": set_speed,
|
||||
"DISTANCE_SETTING": 4,
|
||||
"ACCMode": 0 if not enabled else (2 if gas_override else 1),
|
||||
"MainMode_ACC": 1,
|
||||
"StopReq": 1 if stopping else 0,
|
||||
"aReqValue": a_val,
|
||||
"aReqRaw": a_raw,
|
||||
"VSetDis": set_speed,
|
||||
"JerkLowerLimit": jerk if enabled else 1,
|
||||
|
||||
"ACC_ObjDist": 1,
|
||||
"ObjValid": 1,
|
||||
"ObjValid": 0,
|
||||
"OBJ_STATUS": 2,
|
||||
"SET_ME_2": 0x2,
|
||||
"SET_ME_2": 0x4,
|
||||
"SET_ME_3": 0x3,
|
||||
"SET_ME_TMP_64": 0x64,
|
||||
|
||||
"NEW_SIGNAL_9": 2,
|
||||
"NEW_SIGNAL_10": 4,
|
||||
"DISTANCE_SETTING": 4,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("CRUISE_INFO", get_e_can_bus(CP), values)
|
||||
return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values)
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
from cereal import car
|
||||
from panda import Panda
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
|
||||
from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
|
||||
from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
|
||||
from selfdrive.car import STD_CARGO_KG, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
@@ -42,6 +42,11 @@ class CarInterface(CarInterfaceBase):
|
||||
# non-HDA2
|
||||
if 0x1cf not in fingerprint[4]:
|
||||
ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
|
||||
# ICE cars do not have 0x130; GEARS message on 0x40 instead
|
||||
if 0x130 not in fingerprint[4]:
|
||||
ret.flags |= HyundaiFlags.CANFD_ALT_GEARS.value
|
||||
if candidate not in CANFD_RADAR_SCC_CAR:
|
||||
ret.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value
|
||||
|
||||
ret.steerActuatorDelay = 0.1 # Default delay
|
||||
ret.steerLimitTimer = 0.4
|
||||
@@ -121,6 +126,10 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.wheelbase = 2.756
|
||||
ret.steerRatio = 16.
|
||||
tire_stiffness_factor = 0.385
|
||||
elif candidate == CAR.SANTA_CRUZ_1ST_GEN:
|
||||
ret.mass = 1870. + STD_CARGO_KG # weight from Limited trim - the only supported trim
|
||||
ret.wheelbase = 3.000
|
||||
ret.steerRatio = 14.2 # steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf
|
||||
|
||||
# Kia
|
||||
elif candidate == CAR.KIA_SORENTO:
|
||||
@@ -139,6 +148,10 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.wheelbase = 2.63
|
||||
ret.steerRatio = 14.56
|
||||
tire_stiffness_factor = 1
|
||||
elif candidate == CAR.KIA_SPORTAGE_5TH_GEN:
|
||||
ret.mass = 1700. + STD_CARGO_KG # weight from SX and above trims, average of FWD and AWD versions
|
||||
ret.wheelbase = 2.756
|
||||
ret.steerRatio = 13.6 # steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications
|
||||
elif candidate in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H):
|
||||
ret.mass = 3558. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.80
|
||||
@@ -146,7 +159,7 @@ class CarInterface(CarInterfaceBase):
|
||||
tire_stiffness_factor = 0.5
|
||||
if candidate == CAR.KIA_OPTIMA_G4:
|
||||
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
|
||||
elif candidate == CAR.KIA_STINGER:
|
||||
elif candidate in (CAR.KIA_STINGER, CAR.KIA_STINGER_2022):
|
||||
ret.mass = 1825. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
|
||||
@@ -190,6 +203,10 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.mass = 3673.0 * CV.LB_TO_KG + STD_CARGO_KG
|
||||
ret.wheelbase = 2.83
|
||||
ret.steerRatio = 12.9
|
||||
elif candidate == CAR.GENESIS_GV70_1ST_GEN:
|
||||
ret.mass = 1950. + STD_CARGO_KG
|
||||
ret.wheelbase = 2.87
|
||||
ret.steerRatio = 14.6
|
||||
elif candidate == CAR.GENESIS_G80:
|
||||
ret.mass = 2060. + STD_CARGO_KG
|
||||
ret.wheelbase = 3.01
|
||||
@@ -203,14 +220,10 @@ class CarInterface(CarInterfaceBase):
|
||||
if candidate in CANFD_CAR:
|
||||
ret.longitudinalTuning.kpV = [0.1]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
ret.longitudinalActuatorDelayLowerBound = 0.15
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.5
|
||||
ret.experimentalLongitudinalAvailable = bool(ret.flags & HyundaiFlags.CANFD_HDA2)
|
||||
ret.experimentalLongitudinalAvailable = candidate in (HYBRID_CAR | EV_CAR) and candidate not in CANFD_RADAR_SCC_CAR
|
||||
else:
|
||||
ret.longitudinalTuning.kpV = [0.5]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
ret.longitudinalActuatorDelayLowerBound = 0.5
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.5
|
||||
ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR)
|
||||
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
@@ -219,6 +232,8 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.startingState = True
|
||||
ret.vEgoStarting = 0.1
|
||||
ret.startAccel = 2.0
|
||||
ret.longitudinalActuatorDelayLowerBound = 0.5
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.5
|
||||
|
||||
# *** feature detection ***
|
||||
if candidate in CANFD_CAR:
|
||||
@@ -239,6 +254,8 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_HDA2
|
||||
if ret.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
|
||||
ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CANFD_ALT_BUTTONS
|
||||
if ret.flags & HyundaiFlags.CANFD_CAMERA_SCC:
|
||||
ret.safetyConfigs[1].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
|
||||
else:
|
||||
if candidate in LEGACY_SAFETY_MODE_CAR:
|
||||
# these cars require a special panda safety mode due to missing counters and checksums in the messages
|
||||
@@ -274,7 +291,7 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
@staticmethod
|
||||
def init(CP, logcan, sendcan):
|
||||
if CP.openpilotLongitudinalControl:
|
||||
if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value):
|
||||
addr, bus = 0x7d0, 0
|
||||
if CP.flags & HyundaiFlags.CANFD_HDA2.value:
|
||||
addr, bus = 0x730, 5
|
||||
|
||||
@@ -47,6 +47,8 @@ class CarControllerParams:
|
||||
class HyundaiFlags(IntFlag):
|
||||
CANFD_HDA2 = 1
|
||||
CANFD_ALT_BUTTONS = 2
|
||||
CANFD_ALT_GEARS = 4
|
||||
CANFD_CAMERA_SCC = 8
|
||||
|
||||
|
||||
class CAR:
|
||||
@@ -77,6 +79,7 @@ class CAR:
|
||||
SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021"
|
||||
IONIQ_5 = "HYUNDAI IONIQ 5 2022"
|
||||
TUCSON_HYBRID_4TH_GEN = "HYUNDAI TUCSON HYBRID 4TH GEN"
|
||||
SANTA_CRUZ_1ST_GEN = "HYUNDAI SANTA CRUZ 1ST GEN"
|
||||
|
||||
# Kia
|
||||
KIA_FORTE = "KIA FORTE E 2018 & GT 2021"
|
||||
@@ -88,15 +91,18 @@ class CAR:
|
||||
KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT"
|
||||
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
|
||||
KIA_SELTOS = "KIA SELTOS 2021"
|
||||
KIA_SPORTAGE_5TH_GEN = "KIA SPORTAGE 5TH GEN"
|
||||
KIA_SORENTO = "KIA SORENTO GT LINE 2018"
|
||||
KIA_SPORTAGE_HYBRID_5TH_GEN = "KIA SPORTAGE HYBRID 5TH GEN"
|
||||
KIA_STINGER = "KIA STINGER GT2 2018"
|
||||
KIA_STINGER_2022 = "KIA STINGER 2022"
|
||||
KIA_CEED = "KIA CEED INTRO ED 2019"
|
||||
KIA_EV6 = "KIA EV6 2022"
|
||||
|
||||
# Genesis
|
||||
GENESIS_G70 = "GENESIS G70 2018"
|
||||
GENESIS_G70_2020 = "GENESIS G70 2020"
|
||||
GENESIS_GV70_1ST_GEN = "GENESIS GV70 1ST GEN"
|
||||
GENESIS_G80 = "GENESIS G80 2017"
|
||||
GENESIS_G90 = "GENESIS G90 2017"
|
||||
|
||||
@@ -137,15 +143,16 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
|
||||
],
|
||||
CAR.PALISADE: [
|
||||
HyundaiCarInfo("Hyundai Palisade 2020-22", "All", "https://youtu.be/TAnDqjF4fDY?t=456", harness=Harness.hyundai_h),
|
||||
HyundaiCarInfo("Kia Telluride 2020", "All", harness=Harness.hyundai_h),
|
||||
HyundaiCarInfo("Kia Telluride 2020-22", "All", harness=Harness.hyundai_h),
|
||||
],
|
||||
CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, harness=Harness.hyundai_e),
|
||||
CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", harness=Harness.hyundai_a),
|
||||
CAR.IONIQ_5: [
|
||||
HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022" , "Highway Driving Assist", harness=Harness.hyundai_k),
|
||||
HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022", "Highway Driving Assist II", harness=Harness.hyundai_q),
|
||||
HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23" , "Highway Driving Assist", harness=Harness.hyundai_k),
|
||||
HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", harness=Harness.hyundai_q),
|
||||
],
|
||||
CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022", "All", harness=Harness.hyundai_n),
|
||||
CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2021-22", "Smart Cruise Control (SCC)", harness=Harness.hyundai_n),
|
||||
|
||||
# Kia
|
||||
CAR.KIA_FORTE: HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g),
|
||||
@@ -168,12 +175,14 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
|
||||
HyundaiCarInfo("Kia Optima Hybrid 2019"),
|
||||
],
|
||||
CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness=Harness.hyundai_a),
|
||||
CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", "Smart Cruise Control (SCC)", harness=Harness.hyundai_n),
|
||||
CAR.KIA_SORENTO: [
|
||||
HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", "https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_c),
|
||||
HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_e),
|
||||
],
|
||||
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", harness=Harness.hyundai_n),
|
||||
CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness=Harness.hyundai_c),
|
||||
CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022", "All", harness=Harness.hyundai_k),
|
||||
CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness=Harness.hyundai_e),
|
||||
CAR.KIA_EV6: [
|
||||
HyundaiCarInfo("Kia EV6 (without HDA II) 2022", "Highway Driving Assist", harness=Harness.hyundai_l),
|
||||
@@ -183,6 +192,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
|
||||
# Genesis
|
||||
CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness=Harness.hyundai_f),
|
||||
CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness=Harness.hyundai_f),
|
||||
CAR.GENESIS_GV70_1ST_GEN: HyundaiCarInfo("Genesis GV70 2022-23", "All", harness=Harness.hyundai_l),
|
||||
CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2017-19", "All", harness=Harness.hyundai_h),
|
||||
CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness=Harness.hyundai_c),
|
||||
}
|
||||
@@ -460,6 +470,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x82DNBWN5TMDCXXXG2E',
|
||||
b'\xf1\x82DNCVN5GMCCXXXF0A',
|
||||
b'\xf1\x82DNCVN5GMCCXXXG2B',
|
||||
b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_J10',
|
||||
b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DNDWN5TMDCXXXJ1A',
|
||||
b'\xf1\x87391162M003',
|
||||
b'\xf1\x87391162M013',
|
||||
@@ -484,6 +495,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101',
|
||||
b'\xf1\x8756310L0210\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC101',
|
||||
b'\xf1\x8757700-L0000\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100',
|
||||
b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP101',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422',
|
||||
@@ -504,6 +516,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92',
|
||||
b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB2\x00\x00\x00\x00\x00\x00\x08\xc9O:',
|
||||
b'\xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5',
|
||||
b'\xf1\x00T02601BL T02832A1 VDN8T25XXX832NS8G\x0e\xfeE',
|
||||
b'\xf1\x87954A02N060\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5',
|
||||
b'\xf1\x87SAKFBA2926554GJ2VefVww\x87xwwwww\x88\x87xww\x87wTo\xfb\xffvUo\xff\x8d\x16\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
|
||||
b'\xf1\x87SAKFBA3030524GJ2UVugww\x97yx\x88\x87\x88vw\x87gww\x87wto\xf9\xfffUo\xff\xa2\x0c\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
|
||||
@@ -779,6 +792,23 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0',
|
||||
],
|
||||
},
|
||||
CAR.KIA_STINGER_2022: {
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00CK__ SCC F-CUP 1.00 1.00 99110-J5500 ',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xf1\x81640R0051\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
b'\xf1\x00CK MDPS R 1.00 5.03 57700-J5380 4C2VR503',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00CK MFC AT AUS RHD 1.00 1.00 99211-J5500 210622',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x87VCNLF11383972DK1vffV\x99\x99\x89\x98\x86eUU\x88wg\x89vfff\x97fff\x99\x87o\xff"\xc1\xf1\x81E30\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E30\x00\x00\x00\x00\x00\x00\x00SCK0T33GH0\xbe`\xfb\xc6',
|
||||
],
|
||||
},
|
||||
CAR.PALISADE: {
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00LX2_ SCC F-CUP 1.00 1.04 99110-S8100 ',
|
||||
@@ -1052,6 +1082,7 @@ FW_VERSIONS = {
|
||||
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',
|
||||
b'\xf1\x00OS IEB \r 102"\x05\x16 58520-K4010',
|
||||
# TODO: these return from the MULTI request, above return from LONG
|
||||
b'\x01\x04\x7f\xff\xff\xf8\xff\xff\x00\x00\x01\xd3\x00\x00\x00\x00\xff\xb7\xff\xee\xff\xe0\x00\xc0\xc0\xfc\xd5\xfc\x00\x00U\x10\xffP\xf5\xff\xfd\x00\x00\x00\x00\xfc\x00\x01',
|
||||
b'\x01\x04\x7f\xff\xff\xf8\xff\xff\x00\x00\x01\xdb\x00\x00\x00\x00\xff\xb1\xff\xd9\xff\xd2\x00\xc0\xc0\xfc\xd5\xfc\x00\x00U\x10\xff\xd6\xf5\x00\x06\x00\x00\x00\x14\xfd\x00\x04',
|
||||
@@ -1061,10 +1092,12 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00OSP LKA AT CND LHD 1.00 1.02 99211-J9110 802',
|
||||
b'\xf1\x00OSP LKA AT EUR RHD 1.00 1.02 99211-J9110 802',
|
||||
b'\xf1\x00OSP LKA AT AUS RHD 1.00 1.04 99211-J9200 904',
|
||||
b'\xf1\x00OSP LKA AT EUR LHD 1.00 1.04 99211-J9200 904',
|
||||
],
|
||||
(Ecu.eps, 0x7D4, None): [
|
||||
b'\xf1\x00OSP MDPS C 1.00 1.02 56310K4260\x00 4OEPC102',
|
||||
b'\xf1\x00OSP MDPS C 1.00 1.02 56310/K4970 4OEPC102',
|
||||
b'\xf1\x00OSP MDPS C 1.00 1.02 56310/K4271 4OEPC102',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7D0, None): [
|
||||
b'\xf1\x00YB__ FCA ----- 1.00 1.01 99110-K4500 \x00\x00\x00',
|
||||
@@ -1232,6 +1265,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106',
|
||||
b'\xf1\x8756310/AA070\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA070 4CNDC106',
|
||||
b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106',
|
||||
b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106\xf1\xa01.06',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-AB000 200819',
|
||||
@@ -1244,6 +1278,7 @@ FW_VERSIONS = {
|
||||
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',
|
||||
b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 105 \x10\x03 58910-AA800',
|
||||
b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800\xf1\xa01.01',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x00HT6WA280BLHT6VA640A1CCN0N20NS5\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
@@ -1364,6 +1399,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.05 99210-CV000 211027',
|
||||
b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.06 99210-CV000 220328',
|
||||
b'\xf1\x00CV1 MFC AT EUR LHD 1.00 1.05 99210-CV000 211027',
|
||||
b'\xf1\x00CV1 MFC AT EUR LHD 1.00 1.06 99210-CV000 220328',
|
||||
],
|
||||
},
|
||||
CAR.IONIQ_5: {
|
||||
@@ -1374,11 +1410,13 @@ 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',
|
||||
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614',
|
||||
],
|
||||
},
|
||||
CAR.TUCSON_HYBRID_4TH_GEN: {
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9240 14Q',
|
||||
b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9220 14K',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00NX4__ 1.00 1.00 99110-N9100 ',
|
||||
@@ -1392,6 +1430,32 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00NQ5__ 1.01 1.03 99110-CH000 ',
|
||||
],
|
||||
},
|
||||
CAR.SANTA_CRUZ_1ST_GEN: {
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW000 14M',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00NX4__ 1.00 1.00 99110-K5000 ',
|
||||
],
|
||||
},
|
||||
CAR.KIA_SPORTAGE_5TH_GEN: {
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1030 662',
|
||||
b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1040 663',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00NQ5__ 1.00 1.02 99110-P1000 ',
|
||||
b'\xf1\x00NQ5__ 1.00 1.03 99110-P1000 ',
|
||||
],
|
||||
},
|
||||
CAR.GENESIS_GV70_1ST_GEN: {
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.04 99211-AR000 210204',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00JK1_ SCC FHCUP 1.00 1.02 99110-AR000 ',
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
CHECKSUM = {
|
||||
@@ -1406,10 +1470,13 @@ FEATURES = {
|
||||
"use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KONA_EV_2022},
|
||||
|
||||
# these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
|
||||
"use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022},
|
||||
"use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON, CAR.KONA_EV_2022, CAR.KIA_STINGER_2022},
|
||||
}
|
||||
|
||||
CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN}
|
||||
CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN}
|
||||
|
||||
# The radar does SCC on these cars when HDA I, rather than the camera
|
||||
CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, }
|
||||
|
||||
# The camera does SCC on these cars, rather than the radar
|
||||
CAMERA_SCC_CAR = {CAR.KONA_EV_2022, }
|
||||
@@ -1448,6 +1515,7 @@ DBC = {
|
||||
CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
|
||||
CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KIA_STINGER_2022: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KONA: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.KONA_EV_2022: dbc_dict('hyundai_kia_generic', None),
|
||||
@@ -1466,5 +1534,8 @@ DBC = {
|
||||
CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'),
|
||||
CAR.TUCSON_HYBRID_4TH_GEN: dbc_dict('hyundai_canfd', None),
|
||||
CAR.IONIQ_5: dbc_dict('hyundai_canfd', None),
|
||||
CAR.SANTA_CRUZ_1ST_GEN: dbc_dict('hyundai_canfd', None),
|
||||
CAR.KIA_SPORTAGE_5TH_GEN: dbc_dict('hyundai_canfd', None),
|
||||
CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: dbc_dict('hyundai_canfd', None),
|
||||
CAR.GENESIS_GV70_1ST_GEN: dbc_dict('hyundai_canfd', None),
|
||||
}
|
||||
|
||||
+17
-20
@@ -15,6 +15,7 @@ from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_deadzone
|
||||
from selfdrive.controls.lib.events import Events
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from common.params import Params
|
||||
from selfdrive.car.lat_controller_helper import configure_pid_tune, configure_lqr_tune
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
GearShifter = car.CarState.GearShifter
|
||||
@@ -24,7 +25,7 @@ TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqu
|
||||
MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS
|
||||
ACCEL_MAX = 2.0
|
||||
ACCEL_MIN = -3.5
|
||||
FRICTION_THRESHOLD = 0.2
|
||||
FRICTION_THRESHOLD = 0.3
|
||||
|
||||
TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.yaml')
|
||||
TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.yaml')
|
||||
@@ -161,19 +162,6 @@ class CarInterfaceBase(ABC):
|
||||
ret.steerLimitTimer = 1.0
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def configure_lqr_tune(tune):
|
||||
tune.init('lqr')
|
||||
tune.lqr.scale = 1500.0
|
||||
tune.lqr.ki = 0.05
|
||||
|
||||
tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
|
||||
tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
|
||||
tune.lqr.c = [1., 0.]
|
||||
tune.lqr.k = [-110.73572306, 451.22718255]
|
||||
tune.lqr.l = [0.3233671, 0.3185757]
|
||||
tune.lqr.dcGain = 0.002237852961363602
|
||||
|
||||
@staticmethod
|
||||
def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
|
||||
params = get_torque_params(candidate)
|
||||
@@ -190,10 +178,19 @@ class CarInterfaceBase(ABC):
|
||||
|
||||
@staticmethod
|
||||
def configure_dp_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
|
||||
params = Params()
|
||||
if params.get_bool('dp_lateral_lqr'):
|
||||
CarInterfaceBase.configure_lqr_tune(tune)
|
||||
elif params.get_bool('dp_lateral_torque'):
|
||||
try:
|
||||
dp_lateral_tune = int(Params().get("dp_lateral_tune").decode('utf-8'))
|
||||
except:
|
||||
dp_lateral_tune = 0
|
||||
|
||||
# pid - car specific
|
||||
if dp_lateral_tune == 1:
|
||||
configure_pid_tune(candidate, tune)
|
||||
# lqr - all uses RAV4 one
|
||||
elif dp_lateral_tune == 2:
|
||||
configure_lqr_tune(candidate, tune)
|
||||
# torque - car specific as per lookup table
|
||||
elif dp_lateral_tune == 3:
|
||||
CarInterfaceBase.configure_torque_tune(candidate, tune, steering_angle_deadzone_deg, use_steering_angle)
|
||||
|
||||
@abstractmethod
|
||||
@@ -277,8 +274,8 @@ class CarInterfaceBase(ABC):
|
||||
# Enable OP long on falling edge of enable buttons (defaults to accelCruise and decelCruise, overridable per-port)
|
||||
if not self.CP.pcmCruise and (b.type in enable_buttons and not b.pressed):
|
||||
events.add(EventName.buttonEnable)
|
||||
# Disable on rising edge of cancel for both stock and OP long
|
||||
if b.type == ButtonType.cancel and b.pressed:
|
||||
# Disable on rising and falling edge of cancel for both stock and OP long
|
||||
if b.type == ButtonType.cancel:
|
||||
events.add(EventName.buttonCancel)
|
||||
|
||||
# Handle permanent and temporary steering faults
|
||||
|
||||
@@ -0,0 +1,225 @@
|
||||
'''
|
||||
|
||||
dp - we create a separate controller helper to restore PID/LQR steering tune.
|
||||
'''
|
||||
|
||||
from selfdrive.car.tunes import set_lat_tune, LatTunes
|
||||
from selfdrive.car.toyota.values import CAR as TOYOTA
|
||||
from selfdrive.car.hyundai.values import CAR as HYUNDAI
|
||||
from selfdrive.car.volkswagen.values import CAR as VW
|
||||
from selfdrive.car.subaru.values import CAR as SUBARU
|
||||
|
||||
from common.params import Params
|
||||
|
||||
def configure_pid_tune(candidate, tune):
|
||||
# toyota
|
||||
if candidate == TOYOTA.PRIUS:
|
||||
# indi only
|
||||
pass
|
||||
elif candidate == TOYOTA.PRIUS_V:
|
||||
# lqr only
|
||||
pass
|
||||
elif candidate in (TOYOTA.RAV4, TOYOTA.RAV4H):
|
||||
# lqr only
|
||||
pass
|
||||
elif candidate == TOYOTA.COROLLA:
|
||||
set_lat_tune(tune, LatTunes.PID_A)
|
||||
elif candidate in (TOYOTA.LEXUS_RX, TOYOTA.LEXUS_RXH, TOYOTA.LEXUS_RX_TSS2, TOYOTA.LEXUS_RXH_TSS2):
|
||||
set_lat_tune(tune, LatTunes.PID_C)
|
||||
elif candidate in (TOYOTA.CHR, TOYOTA.CHRH, TOYOTA.CHR_TSS2):
|
||||
set_lat_tune(tune, LatTunes.PID_F)
|
||||
elif candidate in (TOYOTA.CAMRY, TOYOTA.CAMRYH, TOYOTA.CAMRY_TSS2, TOYOTA.CAMRYH_TSS2):
|
||||
if candidate not in (TOYOTA.CAMRY_TSS2, TOYOTA.CAMRYH_TSS2):
|
||||
set_lat_tune(tune, LatTunes.PID_C)
|
||||
elif candidate in (TOYOTA.HIGHLANDER, TOYOTA.HIGHLANDERH, TOYOTA.HIGHLANDER_TSS2, TOYOTA.HIGHLANDERH_TSS2):
|
||||
set_lat_tune(tune, LatTunes.PID_G)
|
||||
elif candidate in (TOYOTA.AVALON, TOYOTA.AVALON_2019, TOYOTA.AVALONH_2019, TOYOTA.AVALON_TSS2, TOYOTA.AVALONH_TSS2):
|
||||
set_lat_tune(tune, LatTunes.PID_H)
|
||||
elif candidate in (TOYOTA.RAV4_TSS2, TOYOTA.RAV4_TSS2_2022, TOYOTA.RAV4H_TSS2, TOYOTA.RAV4H_TSS2_2022):
|
||||
# 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary.
|
||||
if Params().get_bool("dp_toyota_rav4_tss2_tune"):
|
||||
set_lat_tune(tune, LatTunes.PID_I)
|
||||
else:
|
||||
set_lat_tune(tune, LatTunes.PID_D)
|
||||
elif candidate in (TOYOTA.COROLLA_TSS2, TOYOTA.COROLLAH_TSS2):
|
||||
set_lat_tune(tune, LatTunes.PID_D)
|
||||
elif candidate in (TOYOTA.LEXUS_ES_TSS2, TOYOTA.LEXUS_ESH_TSS2, TOYOTA.LEXUS_ESH):
|
||||
set_lat_tune(tune, LatTunes.PID_D)
|
||||
elif candidate == TOYOTA.SIENNA:
|
||||
set_lat_tune(tune, LatTunes.PID_J)
|
||||
elif candidate in (TOYOTA.LEXUS_IS, TOYOTA.LEXUS_RC):
|
||||
set_lat_tune(tune, LatTunes.PID_L)
|
||||
elif candidate == TOYOTA.LEXUS_CTH:
|
||||
set_lat_tune(tune, LatTunes.PID_M)
|
||||
elif candidate in (TOYOTA.LEXUS_NX, TOYOTA.LEXUS_NXH, TOYOTA.LEXUS_NX_TSS2, TOYOTA.LEXUS_NXH_TSS2):
|
||||
set_lat_tune(tune, LatTunes.PID_C)
|
||||
elif candidate == TOYOTA.PRIUS_TSS2:
|
||||
set_lat_tune(tune, LatTunes.PID_N)
|
||||
elif candidate == TOYOTA.MIRAI:
|
||||
set_lat_tune(tune, LatTunes.PID_C)
|
||||
elif candidate in (TOYOTA.ALPHARD_TSS2, TOYOTA.ALPHARDH_TSS2):
|
||||
set_lat_tune(tune, LatTunes.PID_J)
|
||||
|
||||
# hyundai
|
||||
elif candidate in (HYUNDAI.SANTA_FE, HYUNDAI.SANTA_FE_2022, HYUNDAI.SANTA_FE_HEV_2022, HYUNDAI.SANTA_FE_PHEV_2022):
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_D)
|
||||
elif candidate in (HYUNDAI.SONATA, HYUNDAI.SONATA_HYBRID):
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
elif candidate == HYUNDAI.SONATA_LF:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
elif candidate == HYUNDAI.PALISADE:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_C)
|
||||
elif candidate == HYUNDAI.ELANTRA:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_B)
|
||||
elif candidate == HYUNDAI.ELANTRA_2021:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
elif candidate == HYUNDAI.ELANTRA_HEV_2021:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
elif candidate == HYUNDAI.HYUNDAI_GENESIS:
|
||||
# indi only
|
||||
pass
|
||||
elif candidate in (HYUNDAI.KONA, HYUNDAI.KONA_EV, HYUNDAI.KONA_HEV):
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
elif candidate in (HYUNDAI.IONIQ, HYUNDAI.IONIQ_EV_LTD, HYUNDAI.IONIQ_EV_2020, HYUNDAI.IONIQ_PHEV, HYUNDAI.IONIQ_HEV_2022):
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_B)
|
||||
elif candidate == HYUNDAI.IONIQ_PHEV_2019:
|
||||
# indi only
|
||||
pass
|
||||
elif candidate == HYUNDAI.VELOSTER:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
|
||||
# Kia
|
||||
elif candidate == HYUNDAI.KIA_SORENTO:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
elif candidate in (HYUNDAI.KIA_NIRO_EV, HYUNDAI.KIA_NIRO_HEV_2021):
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_B)
|
||||
elif candidate == HYUNDAI.KIA_SELTOS:
|
||||
# indi only
|
||||
pass
|
||||
elif candidate == HYUNDAI.KIA_OPTIMA_H:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
elif candidate == HYUNDAI.KIA_STINGER:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
elif candidate == HYUNDAI.KIA_FORTE:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
elif candidate == HYUNDAI.KIA_CEED:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
elif candidate == HYUNDAI.KIA_K5_2021:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_A)
|
||||
|
||||
# Genesis
|
||||
elif candidate == HYUNDAI.GENESIS_G70:
|
||||
# indi only
|
||||
pass
|
||||
elif candidate == HYUNDAI.GENESIS_G70_2020:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_E)
|
||||
elif candidate == HYUNDAI.GENESIS_G80:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_F)
|
||||
elif candidate == HYUNDAI.GENESIS_G90:
|
||||
set_lat_tune(tune, LatTunes.PID_HYUNDAI_G)
|
||||
|
||||
# VW
|
||||
elif candidate == VW.ARTEON_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.ATLAS_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.GOLF_MK7:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.JETTA_MK7:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.PASSAT_MK8:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.PASSAT_NMS:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.POLO_MK6:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.SHARAN_MK2:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.TAOS_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.TCROSS_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.TIGUAN_MK2:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.TOURAN_MK2:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.TRANSPORTER_T61:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.TROC_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.AUDI_A3_MK3:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.AUDI_Q2_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.AUDI_Q3_MK2:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.SEAT_ATECA_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.SEAT_LEON_MK3:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.SKODA_KAMIQ_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.SKODA_KAROQ_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.SKODA_KODIAQ_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.SKODA_OCTAVIA_MK3:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.SKODA_SCALA_MK1:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
elif candidate == VW.SKODA_SUPERB_MK3:
|
||||
set_lat_tune(tune, LatTunes.PID_VW)
|
||||
|
||||
# subaru
|
||||
elif candidate == SUBARU.ASCENT:
|
||||
set_lat_tune(tune, LatTunes.PID_SUBARU_A)
|
||||
elif candidate == SUBARU.IMPREZA:
|
||||
set_lat_tune(tune, LatTunes.PID_SUBARU_B)
|
||||
elif candidate == SUBARU.IMPREZA_2020:
|
||||
set_lat_tune(tune, LatTunes.PID_SUBARU_C)
|
||||
elif candidate == SUBARU.FORESTER:
|
||||
set_lat_tune(tune, LatTunes.PID_SUBARU_D)
|
||||
elif candidate in (SUBARU.OUTBACK, SUBARU.LEGACY):
|
||||
# torque only
|
||||
pass
|
||||
elif candidate in (SUBARU.FORESTER_PREGLOBAL, SUBARU.OUTBACK_PREGLOBAL_2018):
|
||||
set_lat_tune(tune, LatTunes.PID_SUBARU_E)
|
||||
elif candidate == SUBARU.LEGACY_PREGLOBAL:
|
||||
set_lat_tune(tune, LatTunes.PID_SUBARU_F)
|
||||
elif candidate == SUBARU.OUTBACK_PREGLOBAL:
|
||||
set_lat_tune(tune, LatTunes.PID_SUBARU_E)
|
||||
|
||||
|
||||
'''
|
||||
from RAV4
|
||||
'''
|
||||
def configure_lqr_tune(candidate, tune):
|
||||
tune.init('lqr')
|
||||
tune.lqr.scale = 1500.0
|
||||
tune.lqr.ki = 0.05
|
||||
|
||||
tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
|
||||
tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
|
||||
tune.lqr.c = [1., 0.]
|
||||
tune.lqr.k = [-110.73572306, 451.22718255]
|
||||
tune.lqr.l = [0.3233671, 0.3185757]
|
||||
tune.lqr.dcGain = 0.002237852961363602
|
||||
|
||||
# '''
|
||||
# directly copy from CarInterface.configure_torque_tune
|
||||
# '''
|
||||
# def config_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
|
||||
# try:
|
||||
# params = get_torque_params(candidate)
|
||||
#
|
||||
# tune.init('torque')
|
||||
# tune.torque.useSteeringAngle = use_steering_angle
|
||||
# tune.torque.kp = 1.0
|
||||
# tune.torque.kf = 1.0
|
||||
# tune.torque.ki = 0.1
|
||||
# tune.torque.friction = params['FRICTION']
|
||||
# tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
|
||||
# tune.torque.latAccelOffset = 0.0
|
||||
# tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
|
||||
# except:
|
||||
# pass
|
||||
@@ -44,7 +44,7 @@ class CarState(CarStateBase):
|
||||
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
|
||||
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = ret.vEgoRaw < 0.01
|
||||
ret.standstill = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] == 0.0 and cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] == 0.0
|
||||
|
||||
if self.CP.carFingerprint == CAR.ALTIMA:
|
||||
ret.cruiseState.enabled = bool(cp.vl["CRUISE_STATE"]["CRUISE_ENABLED"])
|
||||
|
||||
@@ -196,6 +196,7 @@ FW_VERSIONS = {
|
||||
b'\xaa!dt\a',
|
||||
b'\xc5!ar\a',
|
||||
b'\xbe!as\a',
|
||||
b'\xc5!as\x07',
|
||||
b'\xc5!ds\a',
|
||||
b'\xc5!`s\a',
|
||||
b'\xaa!au\a',
|
||||
@@ -460,6 +461,7 @@ FW_VERSIONS = {
|
||||
b'\xa1 \x08\x02',
|
||||
b'\xa1 \x06\x02',
|
||||
b'\xa1 \x08\x00',
|
||||
b'\xa1 "\t\x00',
|
||||
],
|
||||
(Ecu.eps, 0x746, None): [
|
||||
b'\x9b\xc0\x10\x00',
|
||||
@@ -481,6 +483,7 @@ FW_VERSIONS = {
|
||||
b'\xe2"`p\x07',
|
||||
b'\xf1\x82\xe2,\xa0@\x07',
|
||||
b'\xbc"`q\x07',
|
||||
b'\xe3,\xa0@\x07',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xa5\xfe\xf7@\x00',
|
||||
|
||||
@@ -99,8 +99,8 @@ BUTTONS = [
|
||||
Button(car.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]),
|
||||
Button(car.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]),
|
||||
Button(car.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]),
|
||||
Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]),
|
||||
Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]),
|
||||
Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]),
|
||||
Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]),
|
||||
]
|
||||
|
||||
class CarControllerParams:
|
||||
|
||||
@@ -34,15 +34,18 @@ class TestCarInterfaces(unittest.TestCase):
|
||||
self.assertGreater(car_params.maxLateralAccel, 0)
|
||||
|
||||
if car_params.steerControlType != car.CarParams.SteerControlType.angle:
|
||||
tuning = car_params.lateralTuning.which()
|
||||
if tuning == 'pid':
|
||||
self.assertTrue(len(car_params.lateralTuning.pid.kpV))
|
||||
elif tuning == 'torque':
|
||||
kf = car_params.lateralTuning.torque.kf
|
||||
self.assertTrue(not math.isnan(kf) and kf > 0)
|
||||
self.assertTrue(not math.isnan(car_params.lateralTuning.torque.friction))
|
||||
elif tuning == 'indi':
|
||||
self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV))
|
||||
tune = car_params.lateralTuning
|
||||
if tune.which() == 'pid':
|
||||
self.assertTrue(not math.isnan(tune.pid.kf) and tune.pid.kf > 0)
|
||||
self.assertTrue(len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP))
|
||||
self.assertTrue(len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP))
|
||||
|
||||
elif tune.which() == 'torque':
|
||||
self.assertTrue(not math.isnan(tune.torque.kf) and tune.torque.kf > 0)
|
||||
self.assertTrue(not math.isnan(tune.torque.friction))
|
||||
|
||||
elif tune.which() == 'indi':
|
||||
self.assertTrue(len(tune.indi.outerLoopGainV))
|
||||
|
||||
# Run car interface
|
||||
CC = car.CarControl.new_message()
|
||||
|
||||
@@ -21,7 +21,6 @@ FORD FOCUS 4TH GEN: [.nan, 1.5, .nan]
|
||||
COMMA BODY: [.nan, 1000, .nan]
|
||||
|
||||
# Totally new cars
|
||||
KIA EV6 2022: [3.5, 3.0, 0.0]
|
||||
RAM 1500 5TH GEN: [2.0, 2.0, 0.0]
|
||||
RAM HD 5TH GEN: [1.4, 1.4, 0.0]
|
||||
SUBARU OUTBACK 6TH GEN: [2.3, 2.3, 0.11]
|
||||
@@ -31,8 +30,10 @@ CHEVROLET SILVERADO 1500 2020: [1.9, 1.9, 0.112]
|
||||
CHEVROLET EQUINOX 2019: [2.0, 2.0, 0.05]
|
||||
VOLKSWAGEN PASSAT NMS: [2.5, 2.5, 0.1]
|
||||
VOLKSWAGEN SHARAN 2ND GEN: [2.5, 2.5, 0.1]
|
||||
HYUNDAI TUCSON HYBRID 4TH GEN: [2.5, 2.5, 0.0]
|
||||
KIA SPORTAGE HYBRID 5TH GEN: [2.5, 2.5, 0.0]
|
||||
HYUNDAI SANTA CRUZ 1ST GEN: [2.7, 2.7, 0.1]
|
||||
KIA SPORTAGE 5TH GEN: [2.7, 2.7, 0.1]
|
||||
KIA SPORTAGE HYBRID 5TH GEN: [2.5, 2.5, 0.1]
|
||||
GENESIS GV70 1ST GEN: [2.42, 2.42, 0.1]
|
||||
|
||||
# Dashcam or fallback configured as ideal car
|
||||
mock: [10.0, 10, 0.0]
|
||||
|
||||
@@ -25,6 +25,7 @@ HONDA ODYSSEY 2018: [1.8774809275211801, 0.8394431662987996, 0.2096978613792822]
|
||||
HONDA PASSPORT 2021: [1.5305538930036766, 0.7956063674638759, 0.19599407381531284]
|
||||
HONDA PILOT 2017: [1.7262026201812795, 0.9470005614967523, 0.21351430733218763]
|
||||
HONDA RIDGELINE 2017: [1.4146525028237624, 0.7356572861629564, 0.23307177552211328]
|
||||
HYUNDAI ELANTRA 2021: [3.169, 2.1259108157250735, 0.0819]
|
||||
HYUNDAI GENESIS 2015-2016: [1.8466226943929824, 1.5552063647830634, 0.0984484465421171]
|
||||
HYUNDAI IONIQ ELECTRIC LIMITED 2019: [1.7662975472852054, 1.613755614526594, 0.17087579756306276]
|
||||
HYUNDAI IONIQ PHEV 2020: [3.2928700076638537, 2.1193482926455656, 0.12463700961468778]
|
||||
@@ -35,21 +36,23 @@ HYUNDAI SANTA FE 2019: [3.0787027729757632, 2.6173437483495565, 0.12070193418239
|
||||
HYUNDAI SANTA FE HYBRID 2022: [3.501877602644835, 2.729064118456137, 0.10384068104538963]
|
||||
HYUNDAI SANTA FE PlUG-IN HYBRID 2022: [1.6953050513611045, 1.5837614296206861, 0.12672855941458458]
|
||||
HYUNDAI SONATA 2019: [2.2200457811703953, 1.2967330275895228, 0.14039920986586393]
|
||||
HYUNDAI SONATA 2020: [3.284505627881726, 2.1259108157250735, 0.08452048323586728]
|
||||
HYUNDAI SONATA 2020: [2.9638737459977467, 2.1259108157250735, 0.07813665616927593]
|
||||
HYUNDAI SONATA HYBRID 2021: [2.8990264092395734, 2.061410192222139, 0.0899805488717382]
|
||||
HYUNDAI TUCSON HYBRID 4TH GEN: [2.035545, 2.035545, 0.110272]
|
||||
JEEP GRAND CHEROKEE 2019: [1.7321233388827006, 1.289689569171081, 0.15046331002097185]
|
||||
JEEP GRAND CHEROKEE V6 2018: [1.8776598027756923, 1.4057367824262523, 0.11725947414922003]
|
||||
KIA EV6 2022: [3.2, 3.0, 0.05]
|
||||
KIA K5 2021: [2.405339728085138, 1.460032270828705, 0.11650989850813716]
|
||||
KIA NIRO EV 2020: [2.9215954981365337, 2.1500583840260044, 0.09236802474810267]
|
||||
KIA SORENTO GT LINE 2018: [2.464854685101844, 1.5335274218367956, 0.12056170567599558]
|
||||
KIA STINGER GT2 2018: [2.7499043387418967, 1.849652021986449, 0.12048334239559202]
|
||||
LEXUS ES 2019: [2.0203086922726112, 2.134803912579666, 0.12757526789308554]
|
||||
LEXUS ES HYBRID 2019: [2.392442298703042, 1.863360677810788, 0.17690002108856212]
|
||||
LEXUS ES 2019: [1.935835, 2.134803912579666, 0.093439]
|
||||
LEXUS ES HYBRID 2019: [2.135678, 1.863360677810788, 0.109627]
|
||||
LEXUS NX 2018: [2.302625600642627, 2.1382378491466625, 0.14986840878892838]
|
||||
LEXUS NX 2020: [2.4331999786982936, 2.1045680431705414, 0.14099899317761067]
|
||||
LEXUS NX HYBRID 2018: [2.4025593501080955, 1.8080446063815507, 0.15349361249519017]
|
||||
LEXUS RX 2016: [1.5876816543130423, 1.0427699298523752, 0.21334066732397142]
|
||||
LEXUS RX 2020: [1.5228812994274734, 1.431102486563665, 0.2093316728710659]
|
||||
LEXUS RX 2020: [1.5228812994274734, 1.431102486563665, 0.164117]
|
||||
LEXUS RX HYBRID 2017: [1.6984261557042386, 1.3211501880159107, 0.1820354534928893]
|
||||
LEXUS RX HYBRID 2020: [1.5522309889823778, 1.255230465866663, 0.2220954003055114]
|
||||
MAZDA CX-9 2021: [1.7601682915983443, 1.0889677335154337, 0.17713792194297195]
|
||||
@@ -62,31 +65,31 @@ TOYOTA AVALON 2019: [1.7036141952825095, 1.239619084240008, 0.08459830394899492]
|
||||
TOYOTA AVALON 2022: [2.3154403649717357, 2.7777922854327124, 0.11453999639164605]
|
||||
TOYOTA C-HR 2018: [1.5591084333664578, 1.271271459066948, 0.20259087058453193]
|
||||
TOYOTA C-HR 2021: [1.7678810166088303, 1.3742176337919942, 0.2319674583741509]
|
||||
TOYOTA CAMRY 2018: [2.1172995371905015, 1.7156177222420887, 0.13519250664782062]
|
||||
TOYOTA CAMRY 2021: [2.6922769557433055, 2.3476510120007434, 0.1450430192989234]
|
||||
TOYOTA CAMRY HYBRID 2018: [2.0974120828287774, 1.7996193116697359, 0.13823613467632756]
|
||||
TOYOTA CAMRY HYBRID 2021: [2.6426668350384457, 2.3901492458927986, 0.16103875108816076]
|
||||
TOYOTA CAMRY 2018: [2.1172995371905015, 1.7156177222420887, 0.105192506]
|
||||
TOYOTA CAMRY 2021: [2.446083, 2.3476510120007434, 0.121615]
|
||||
TOYOTA CAMRY HYBRID 2018: [1.996333, 1.7996193116697359, 0.112565]
|
||||
TOYOTA CAMRY HYBRID 2021: [2.263582, 2.3901492458927986, 0.115257]
|
||||
TOYOTA COROLLA 2017: [3.117154369115421, 1.8438132575043773, 0.12289685869250652]
|
||||
TOYOTA COROLLA HYBRID TSS2 2019: [2.3287672277252005, 1.8118712531729109, 0.2215868445753317]
|
||||
TOYOTA COROLLA TSS2 2019: [2.4204464833010175, 1.9258612322678952, 0.20670411068012526]
|
||||
TOYOTA COROLLA HYBRID TSS2 2019: [1.9079729107361805, 1.8118712531729109, 0.22251440891543514]
|
||||
TOYOTA COROLLA TSS2 2019: [2.0742917676766712, 1.9258612322678952, 0.16888685704519352]
|
||||
TOYOTA HIGHLANDER 2017: [1.8696367437248915, 1.626293990451463, 0.17485372210240796]
|
||||
TOYOTA HIGHLANDER 2020: [2.022340166827233, 1.6183134804881791, 0.14592306380054457]
|
||||
TOYOTA HIGHLANDER HYBRID 2018: [1.9421825202382728, 1.6433903296845025, 0.16928956792275918]
|
||||
TOYOTA HIGHLANDER HYBRID 2020: [2.103373061114133, 2.104015182965606, 0.14447040132184993]
|
||||
TOYOTA HIGHLANDER HYBRID 2018: [1.752033, 1.6433903296845025, 0.144600]
|
||||
TOYOTA HIGHLANDER HYBRID 2020: [1.901174, 2.104015182965606, 0.14447040132184993]
|
||||
TOYOTA MIRAI 2021: [2.506899832157829, 1.7417213930750164, 0.20182618449440565]
|
||||
TOYOTA PRIUS 2017: [2.0183401513314294, 1.5023147650693636, 0.20856908464957724]
|
||||
TOYOTA PRIUS TSS2 2021: [2.327639738920072, 1.9104337425537743, 0.2030762265549664]
|
||||
TOYOTA PRIUS 2017: [1.746445, 1.5023147650693636, 0.151515]
|
||||
TOYOTA PRIUS TSS2 2021: [1.972600, 1.9104337425537743, 0.170968]
|
||||
TOYOTA RAV4 2017: [2.085695074355425, 2.2142832316984733, 0.13339165270103975]
|
||||
TOYOTA RAV4 2019: [2.5038362866776835, 2.0993589721530252, 0.1552425356342368]
|
||||
TOYOTA RAV4 2019: [2.331293, 2.0993589721530252, 0.129822]
|
||||
TOYOTA RAV4 2019 8965: [2.5084506298290377, 2.4216520504763475, 0.11992835265067918]
|
||||
TOYOTA RAV4 2019 x02: [2.7209621987605024, 2.2148637653781593, 0.10862567142268198]
|
||||
TOYOTA RAV4 HYBRID 2017: [1.9796257271652042, 1.7503987331707576, 0.14628860048885406]
|
||||
TOYOTA RAV4 HYBRID 2019: [2.2271858492309153, 2.074844961405639, 0.14382216826893632]
|
||||
TOYOTA RAV4 HYBRID 2019 8965: [2.1077397198131336, 1.8162215166877735, 0.13891369391200137]
|
||||
TOYOTA RAV4 HYBRID 2019 x02: [2.803624333289342, 2.272367966572498, 0.11364569214387774]
|
||||
TOYOTA RAV4 HYBRID 2022: [2.241883248393209, 1.9304407208090029, 0.1565442715453653]
|
||||
TOYOTA RAV4 HYBRID 2022: [2.241883248393209, 1.9304407208090029, 0.112174]
|
||||
TOYOTA RAV4 HYBRID 2022 x02: [3.044930631831037, 2.3979189796380918, 0.14023209146703736]
|
||||
TOYOTA SIENNA 2018: [1.8660896232147548, 1.3208264576110418, 0.18799149615227198]
|
||||
TOYOTA SIENNA 2018: [1.689726, 1.3208264576110418, 0.140456]
|
||||
VOLKSWAGEN ARTEON 1ST GEN: [1.45136518053819, 1.3639364049316804, 0.23806361745695032]
|
||||
VOLKSWAGEN ATLAS 1ST GEN: [1.4677006726964945, 1.6733266634075656, 0.12959584092073367]
|
||||
VOLKSWAGEN GOLF 7TH GEN: [1.3750394140491293, 1.5814743077200641, 0.2018321939386586]
|
||||
|
||||
@@ -35,9 +35,9 @@ HYUNDAI IONIQ HYBRID 2020-2022: HYUNDAI IONIQ PLUG-IN HYBRID 2019
|
||||
HYUNDAI IONIQ ELECTRIC 2020: HYUNDAI IONIQ PLUG-IN HYBRID 2019
|
||||
HYUNDAI ELANTRA 2017: HYUNDAI SONATA 2019
|
||||
HYUNDAI ELANTRA HYBRID 2021: HYUNDAI SONATA 2020
|
||||
HYUNDAI ELANTRA 2021: HYUNDAI SONATA 2020
|
||||
HYUNDAI TUCSON 2019: HYUNDAI SANTA FE 2019
|
||||
HYUNDAI SANTA FE 2022: HYUNDAI SANTA FE HYBRID 2022
|
||||
KIA STINGER 2022: KIA STINGER GT2 2018
|
||||
GENESIS G90 2017: GENESIS G70 2018
|
||||
GENESIS G80 2017: GENESIS G70 2018
|
||||
GENESIS G70 2020: HYUNDAI SONATA 2020
|
||||
|
||||
@@ -5,7 +5,8 @@ from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_comma
|
||||
create_accel_command, create_acc_cancel_command, \
|
||||
create_fcw_command, create_lta_steer_command
|
||||
from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
|
||||
MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
|
||||
MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, \
|
||||
UNSUPPORTED_DSU_CAR
|
||||
from opendbc.can.packer import CANPacker
|
||||
from common.conversions import Conversions as CV
|
||||
|
||||
@@ -150,7 +151,7 @@ class CarController:
|
||||
lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
|
||||
|
||||
# Lexus IS uses a different cancellation message
|
||||
if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
|
||||
if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
|
||||
can_sends.append(create_acc_cancel_command(self.packer))
|
||||
elif self.CP.openpilotLongitudinalControl:
|
||||
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, CS.distance))
|
||||
|
||||
@@ -6,7 +6,8 @@ from common.realtime import DT_CTRL
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE
|
||||
from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
|
||||
|
||||
from common.params import Params, put_nonblocking
|
||||
import time
|
||||
from math import floor
|
||||
@@ -30,6 +31,8 @@ class CarState(CarStateBase):
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"]
|
||||
self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100.
|
||||
self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.
|
||||
self.cluster_min_speed = CV.KPH_TO_MS / 2.
|
||||
|
||||
# On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
|
||||
# the signal is zeroed to where the steering angle is at start.
|
||||
@@ -88,6 +91,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.vEgoCluster = ret.vEgo * 1.015 # minimum of all the cars
|
||||
|
||||
ret.standstill = ret.vEgoRaw == 0
|
||||
|
||||
@@ -194,7 +198,7 @@ class CarState(CarStateBase):
|
||||
# 17 is a fault from a prolonged high torque delta between cmd and user
|
||||
ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] == 17
|
||||
|
||||
if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
|
||||
if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
|
||||
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"]
|
||||
@@ -219,17 +223,12 @@ class CarState(CarStateBase):
|
||||
# these cars are identified by an ACC_TYPE value of 2.
|
||||
# TODO: it is possible to avoid the lockout and gain stop and go if you
|
||||
# send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
|
||||
if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in (CAR.LEXUS_IS, CAR.LEXUS_RC)) or \
|
||||
if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR) or \
|
||||
(self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
|
||||
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
|
||||
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
|
||||
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
|
||||
# ignore standstill in hybrid vehicles, since pcm allows to restart without
|
||||
# receiving any special command. Also if interceptor is detected
|
||||
ret.cruiseState.standstill = False
|
||||
else:
|
||||
ret.cruiseState.standstill = self.pcm_acc_status == 7
|
||||
ret.cruiseState.standstill = self.pcm_acc_status == 7
|
||||
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
|
||||
ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6)
|
||||
|
||||
@@ -403,7 +402,7 @@ class CarState(CarStateBase):
|
||||
signals.append(("SPORT_ON", "GEAR_PACKET"))
|
||||
signals.append(("ECON_ON", "GEAR_PACKET"))
|
||||
|
||||
if CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
|
||||
if CP.carFingerprint in UNSUPPORTED_DSU_CAR:
|
||||
signals.append(("MAIN_ON", "DSU_CRUISE"))
|
||||
signals.append(("SET_SPEED", "DSU_CRUISE"))
|
||||
signals.append(("UI_SET_SPEED", "PCM_CRUISE_ALT"))
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from panda import Panda
|
||||
from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams
|
||||
from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, CarControllerParams, NO_STOP_TIMER_CAR
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
from common.params import Params
|
||||
@@ -125,6 +125,21 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.steerRatio = 14.3
|
||||
tire_stiffness_factor = 0.7933
|
||||
ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kiBP = [0.0]
|
||||
ret.lateralTuning.pid.kpBP = [0.0]
|
||||
ret.lateralTuning.pid.kpV = [0.6]
|
||||
ret.lateralTuning.pid.kiV = [0.1]
|
||||
ret.lateralTuning.pid.kf = 0.00007818594
|
||||
|
||||
# 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary.
|
||||
# See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891
|
||||
for fw in car_fw:
|
||||
if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']):
|
||||
ret.lateralTuning.pid.kpV = [0.15]
|
||||
ret.lateralTuning.pid.kiV = [0.05]
|
||||
ret.lateralTuning.pid.kf = 0.00004
|
||||
break
|
||||
|
||||
elif candidate in (CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2):
|
||||
stop_and_go = True
|
||||
@@ -206,21 +221,22 @@ class CarInterface(CarInterfaceBase):
|
||||
smartDsu = 0x2FF in fingerprint[0]
|
||||
# In TSS2 cars the camera does long control
|
||||
found_ecus = [fw.ecu for fw in car_fw]
|
||||
ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR) and (not smartDsu)
|
||||
ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) and not smartDsu
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[0]
|
||||
# if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
|
||||
ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR)
|
||||
ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
|
||||
|
||||
if int(params.get("dp_atl").decode('utf-8')) == 1:
|
||||
ret.openpilotLongitudinalControl = False
|
||||
|
||||
if smartDsu and int(params.get("dp_atl").decode('utf-8')) == 2:
|
||||
ret.openpilotLongitudinalControl = True
|
||||
|
||||
if candidate == CAR.CHR_TSS2:
|
||||
ret.enableBsm = True
|
||||
if smartDsu and int(params.get("dp_atl").decode('utf-8')) == 2:
|
||||
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
|
||||
@@ -233,9 +249,10 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
|
||||
|
||||
tune = ret.longitudinalTuning
|
||||
tune.deadzoneBP = [0., 9.]
|
||||
tune.deadzoneV = [.0, .15]
|
||||
if candidate in TSS2_CAR or ret.enableGasInterceptor:
|
||||
tune.deadzoneBP = [0., 8.05]
|
||||
tune.deadzoneV = [.0, .14]
|
||||
|
||||
tune.kpBP = [0., 5., 20., 30.]
|
||||
tune.kpV = [1.3, 1.0, 0.7, 0.1]
|
||||
#really smooth (make it toggleable)
|
||||
@@ -254,8 +271,6 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.longitudinalActuatorDelayLowerBound = 0.3
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.3
|
||||
else:
|
||||
tune.deadzoneBP = [0., 9.]
|
||||
tune.deadzoneV = [.0, .15]
|
||||
tune.kpBP = [0., 5., 35.]
|
||||
tune.kiBP = [0., 35.]
|
||||
tune.kpV = [3.6, 2.4, 1.5]
|
||||
@@ -287,16 +302,19 @@ class CarInterface(CarInterfaceBase):
|
||||
events = self.create_common_events(ret)
|
||||
events = self.dp_atl_warning(ret, events)
|
||||
|
||||
if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl:
|
||||
events.add(EventName.lowSpeedLockout)
|
||||
if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl:
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
if c.actuators.accel > 0.3:
|
||||
# some margin on the actuator to not false trigger cancellation while stopping
|
||||
events.add(EventName.speedTooLow)
|
||||
if ret.vEgo < 0.001:
|
||||
# while in standstill, send a user alert
|
||||
events.add(EventName.manualRestart)
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if ret.cruiseState.standstill and not ret.brakePressed and not self.CP.enableGasInterceptor:
|
||||
events.add(EventName.resumeRequired)
|
||||
if self.CS.low_speed_lockout:
|
||||
events.add(EventName.lowSpeedLockout)
|
||||
if ret.vEgo < self.CP.minEnableSpeed:
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
if c.actuators.accel > 0.3:
|
||||
# some margin on the actuator to not false trigger cancellation while stopping
|
||||
events.add(EventName.speedTooLow)
|
||||
if ret.vEgo < 0.001:
|
||||
# while in standstill, send a user alert
|
||||
events.add(EventName.manualRestart)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
|
||||
@@ -88,10 +88,6 @@ class CAR:
|
||||
|
||||
|
||||
class Footnote(Enum):
|
||||
DSU = CarFootnote(
|
||||
"When the Driver Support Unit (DSU) is disconnected, openpilot Adaptive Cruise Control (ACC) will replace " +
|
||||
"stock Adaptive Cruise Control (ACC). <b><i>NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).</i></b>",
|
||||
Column.LONGITUDINAL)
|
||||
CAMRY = CarFootnote(
|
||||
"openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.",
|
||||
Column.FSR_LONGITUDINAL)
|
||||
@@ -108,11 +104,11 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
|
||||
CAR.ALPHARD_TSS2: ToyotaCarInfo("Toyota Alphard 2019-20"),
|
||||
CAR.ALPHARDH_TSS2: ToyotaCarInfo("Toyota Alphard Hybrid 2021"),
|
||||
CAR.AVALON: [
|
||||
ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P", footnotes=[Footnote.DSU]),
|
||||
ToyotaCarInfo("Toyota Avalon 2017-18", footnotes=[Footnote.DSU]),
|
||||
ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P"),
|
||||
ToyotaCarInfo("Toyota Avalon 2017-18"),
|
||||
],
|
||||
CAR.AVALON_2019: ToyotaCarInfo("Toyota Avalon 2019-21", footnotes=[Footnote.DSU]),
|
||||
CAR.AVALONH_2019: ToyotaCarInfo("Toyota Avalon Hybrid 2019-21", footnotes=[Footnote.DSU]),
|
||||
CAR.AVALON_2019: ToyotaCarInfo("Toyota Avalon 2019-21"),
|
||||
CAR.AVALONH_2019: ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"),
|
||||
CAR.AVALON_TSS2: ToyotaCarInfo("Toyota Avalon 2022"),
|
||||
CAR.AVALONH_TSS2: ToyotaCarInfo("Toyota Avalon Hybrid 2022"),
|
||||
CAR.CAMRY: ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]),
|
||||
@@ -121,10 +117,10 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
|
||||
CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-22"),
|
||||
CAR.CHR: ToyotaCarInfo("Toyota C-HR 2017-21"),
|
||||
CAR.CHRH: ToyotaCarInfo("Toyota C-HR Hybrid 2017-19"),
|
||||
CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19", footnotes=[Footnote.DSU]),
|
||||
CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"),
|
||||
CAR.COROLLA_TSS2: [
|
||||
ToyotaCarInfo("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"),
|
||||
ToyotaCarInfo("Toyota Corolla Cross (Non-US only) 2020-21", min_enable_speed=7.5),
|
||||
ToyotaCarInfo("Toyota Corolla Cross (Non-US only) 2020-23", min_enable_speed=7.5),
|
||||
ToyotaCarInfo("Toyota Corolla Hatchback 2019-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"),
|
||||
],
|
||||
CAR.COROLLAH_TSS2: [
|
||||
@@ -132,53 +128,53 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
|
||||
ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5),
|
||||
ToyotaCarInfo("Lexus UX Hybrid 2019-22"),
|
||||
],
|
||||
CAR.HIGHLANDER: ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo", footnotes=[Footnote.DSU]),
|
||||
CAR.HIGHLANDER: ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"),
|
||||
CAR.HIGHLANDER_TSS2: ToyotaCarInfo("Toyota Highlander 2020-22"),
|
||||
CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19", footnotes=[Footnote.DSU]),
|
||||
CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19"),
|
||||
CAR.HIGHLANDERH_TSS2: ToyotaCarInfo("Toyota Highlander Hybrid 2020-22"),
|
||||
CAR.PRIUS: [
|
||||
ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", "https://www.youtube.com/watch?v=8zopPJI8XQ0", [Footnote.DSU]),
|
||||
ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]),
|
||||
ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0", footnotes=[Footnote.DSU]),
|
||||
ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", "https://www.youtube.com/watch?v=8zopPJI8XQ0"),
|
||||
ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
|
||||
ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
|
||||
],
|
||||
CAR.PRIUS_V: ToyotaCarInfo("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED, footnotes=[Footnote.DSU]),
|
||||
CAR.PRIUS_V: ToyotaCarInfo("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED),
|
||||
CAR.PRIUS_TSS2: [
|
||||
ToyotaCarInfo("Toyota Prius 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"),
|
||||
ToyotaCarInfo("Toyota Prius Prime 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"),
|
||||
],
|
||||
CAR.RAV4: [
|
||||
ToyotaCarInfo("Toyota RAV4 2016", "Toyota Safety Sense P", footnotes=[Footnote.DSU]),
|
||||
ToyotaCarInfo("Toyota RAV4 2017-18", footnotes=[Footnote.DSU])
|
||||
ToyotaCarInfo("Toyota RAV4 2016", "Toyota Safety Sense P"),
|
||||
ToyotaCarInfo("Toyota RAV4 2017-18")
|
||||
],
|
||||
CAR.RAV4H: [
|
||||
ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", "https://youtu.be/LhT5VzJVfNI?t=26", [Footnote.DSU]),
|
||||
ToyotaCarInfo("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26", footnotes=[Footnote.DSU])
|
||||
ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", "https://youtu.be/LhT5VzJVfNI?t=26"),
|
||||
ToyotaCarInfo("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26")
|
||||
],
|
||||
CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
|
||||
CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"),
|
||||
CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
|
||||
CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"),
|
||||
CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"),
|
||||
CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", footnotes=[Footnote.DSU], min_enable_speed=MIN_ACC_SPEED),
|
||||
CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED),
|
||||
|
||||
# Lexus
|
||||
CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "Lexus Safety System+", footnotes=[Footnote.DSU]),
|
||||
CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18", "Lexus Safety System+", footnotes=[Footnote.DSU]),
|
||||
CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "Lexus Safety System+"),
|
||||
CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18", "Lexus Safety System+"),
|
||||
CAR.LEXUS_ES_TSS2: ToyotaCarInfo("Lexus ES 2019-22"),
|
||||
CAR.LEXUS_ESH_TSS2: ToyotaCarInfo("Lexus ES Hybrid 2019-22", video_link="https://youtu.be/BZ29osRVJeg?t=12"),
|
||||
CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"),
|
||||
CAR.LEXUS_NX: ToyotaCarInfo("Lexus NX 2018-19", footnotes=[Footnote.DSU]),
|
||||
CAR.LEXUS_NXH: ToyotaCarInfo("Lexus NX Hybrid 2018-19", footnotes=[Footnote.DSU]),
|
||||
CAR.LEXUS_NX: ToyotaCarInfo("Lexus NX 2018-19"),
|
||||
CAR.LEXUS_NXH: ToyotaCarInfo("Lexus NX Hybrid 2018-19"),
|
||||
CAR.LEXUS_NX_TSS2: ToyotaCarInfo("Lexus NX 2020-21"),
|
||||
CAR.LEXUS_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020-21"),
|
||||
CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2017-20"),
|
||||
CAR.LEXUS_RX: [
|
||||
ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+", footnotes=[Footnote.DSU]),
|
||||
ToyotaCarInfo("Lexus RX 2017-19", footnotes=[Footnote.DSU]),
|
||||
ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+"),
|
||||
ToyotaCarInfo("Lexus RX 2017-19"),
|
||||
],
|
||||
CAR.LEXUS_RXH: [
|
||||
ToyotaCarInfo("Lexus RX Hybrid 2016", "Lexus Safety System+", footnotes=[Footnote.DSU]),
|
||||
ToyotaCarInfo("Lexus RX Hybrid 2017-19", footnotes=[Footnote.DSU]),
|
||||
ToyotaCarInfo("Lexus RX Hybrid 2016", "Lexus Safety System+"),
|
||||
ToyotaCarInfo("Lexus RX Hybrid 2017-19"),
|
||||
],
|
||||
CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-22"),
|
||||
CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-21"),
|
||||
@@ -214,16 +210,19 @@ FW_QUERY_CONFIG = FwQueryConfig(
|
||||
Request(
|
||||
[StdQueries.SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST],
|
||||
[StdQueries.SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.dsu, Ecu.abs, Ecu.eps],
|
||||
bus=0,
|
||||
),
|
||||
Request(
|
||||
[StdQueries.SHORT_TESTER_PRESENT_REQUEST, StdQueries.OBD_VERSION_REQUEST],
|
||||
[StdQueries.SHORT_TESTER_PRESENT_RESPONSE, StdQueries.OBD_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.engine],
|
||||
bus=0,
|
||||
),
|
||||
Request(
|
||||
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.DEFAULT_DIAGNOSTIC_REQUEST, StdQueries.EXTENDED_DIAGNOSTIC_REQUEST, StdQueries.UDS_VERSION_REQUEST],
|
||||
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.DEFAULT_DIAGNOSTIC_RESPONSE, StdQueries.EXTENDED_DIAGNOSTIC_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.engine, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.abs, Ecu.eps],
|
||||
bus=0,
|
||||
),
|
||||
],
|
||||
@@ -719,6 +718,7 @@ FW_VERSIONS = {
|
||||
},
|
||||
CAR.COROLLA_TSS2: {
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x01896630A22000\x00\x00\x00\x00',
|
||||
b'\x01896630ZG2000\x00\x00\x00\x00',
|
||||
b'\x01896630ZG5000\x00\x00\x00\x00',
|
||||
b'\x01896630ZG5100\x00\x00\x00\x00',
|
||||
@@ -798,6 +798,7 @@ FW_VERSIONS = {
|
||||
b'F152602191\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F152612862\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F152612B91\x00\x00\x00\x00\x00\x00',
|
||||
b'\x01F15260A070\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
b'\x018821F3301100\x00\x00\x00\x00',
|
||||
@@ -815,6 +816,7 @@ FW_VERSIONS = {
|
||||
b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
|
||||
b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
|
||||
b'\x028646F1601100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
|
||||
b'\x028646F1601300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.COROLLAH_TSS2: {
|
||||
@@ -824,6 +826,7 @@ FW_VERSIONS = {
|
||||
b'\x01896637621000\x00\x00\x00\x00',
|
||||
b'\x01896637624000\x00\x00\x00\x00',
|
||||
b'\x01896637626000\x00\x00\x00\x00',
|
||||
b'\x01896637639000\x00\x00\x00\x00',
|
||||
b'\x01896637648000\x00\x00\x00\x00',
|
||||
b'\x01896637643000\x00\x00\x00\x00',
|
||||
b'\x02896630A07000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
|
||||
@@ -1017,6 +1020,7 @@ FW_VERSIONS = {
|
||||
b'\x01F15264873500\x00\x00\x00\x00',
|
||||
b'\x01F152648C6300\x00\x00\x00\x00',
|
||||
b'\x01F152648J4000\x00\x00\x00\x00',
|
||||
b'\x01F152648J5000\x00\x00\x00\x00',
|
||||
b'\x01F152648J6000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x700, None): [
|
||||
@@ -1024,6 +1028,7 @@ FW_VERSIONS = {
|
||||
b'\x01896630EA1000\x00\x00\x00\x00',
|
||||
b'\x01896630EE4000\x00\x00\x00\x00',
|
||||
b'\x01896630EE4100\x00\x00\x00\x00',
|
||||
b'\x01896630EE5000\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',
|
||||
@@ -1365,10 +1370,11 @@ FW_VERSIONS = {
|
||||
],
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x01896634AA0000\x00\x00\x00\x00',
|
||||
b'\x01896634AA0100\x00\x00\x00\x00',
|
||||
b'\x01896634AA0100\x00\x00\x00\x00',
|
||||
b'\x01896634AA1000\x00\x00\x00\x00',
|
||||
b'\x01896634A88000\x00\x00\x00\x00',
|
||||
b'\x01896634A89000\x00\x00\x00\x00',
|
||||
b'\x01896634A89100\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
b'\x018821F0R01100\x00\x00\x00\x00',
|
||||
@@ -1394,6 +1400,7 @@ FW_VERSIONS = {
|
||||
b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
|
||||
b'\x02896634A23000\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
|
||||
b'\x02896634A23101\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
|
||||
b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
b'\x02896634A14101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
|
||||
],
|
||||
@@ -2034,6 +2041,9 @@ TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TS
|
||||
|
||||
NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
|
||||
|
||||
# the DSU uses the AEB message for longitudinal on these cars
|
||||
UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC}
|
||||
|
||||
# these cars have a radar which sends ACC messages instead of the camera
|
||||
RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.CHR_TSS2}
|
||||
|
||||
|
||||
@@ -0,0 +1,181 @@
|
||||
#!/usr/bin/env python3
|
||||
from enum import Enum
|
||||
|
||||
class LatTunes(Enum):
|
||||
#TOYOTA
|
||||
INDI_PRIUS = 0
|
||||
LQR_RAV4 = 1
|
||||
PID_A = 2
|
||||
PID_B = 3
|
||||
PID_C = 4
|
||||
PID_D = 5
|
||||
PID_E = 6
|
||||
PID_F = 7
|
||||
PID_G = 8
|
||||
PID_I = 9
|
||||
PID_H = 10
|
||||
PID_J = 11
|
||||
PID_K = 12
|
||||
PID_L = 13
|
||||
PID_M = 14
|
||||
PID_N = 15
|
||||
INDI_PRIUS_TSS2 = 16
|
||||
|
||||
#HKG
|
||||
PID_HYUNDAI_A = 17
|
||||
PID_HYUNDAI_B = 18
|
||||
PID_HYUNDAI_C = 19
|
||||
PID_HYUNDAI_D = 20
|
||||
PID_HYUNDAI_E = 21
|
||||
PID_HYUNDAI_F = 22
|
||||
PID_HYUNDAI_G = 23
|
||||
|
||||
#VW
|
||||
PID_VW = 24
|
||||
|
||||
#SUBARU
|
||||
PID_SUBARU_A = 25
|
||||
PID_SUBARU_B = 26
|
||||
PID_SUBARU_C = 27
|
||||
PID_SUBARU_D = 28
|
||||
PID_SUBARU_E = 29
|
||||
PID_SUBARU_F = 30
|
||||
|
||||
###### LAT ######
|
||||
def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0, use_steering_angle=True):
|
||||
|
||||
#TODO: add toggle for sepcial prius_tss2 indi
|
||||
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 = [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]
|
||||
tune.pid.kpBP = [0.0]
|
||||
if name == LatTunes.PID_A:
|
||||
tune.pid.kpV = [0.2]
|
||||
tune.pid.kiV = [0.05]
|
||||
tune.pid.kf = 0.00003
|
||||
elif name == LatTunes.PID_C:
|
||||
tune.pid.kpV = [0.6]
|
||||
tune.pid.kiV = [0.1]
|
||||
tune.pid.kf = 0.00006
|
||||
elif name == LatTunes.PID_D:
|
||||
tune.pid.kpV = [0.6]
|
||||
tune.pid.kiV = [0.1]
|
||||
tune.pid.kf = 0.00007818594
|
||||
elif name == LatTunes.PID_F:
|
||||
tune.pid.kpV = [0.723]
|
||||
tune.pid.kiV = [0.0428]
|
||||
tune.pid.kf = 0.00006
|
||||
elif name == LatTunes.PID_G:
|
||||
tune.pid.kpV = [0.18]
|
||||
tune.pid.kiV = [0.015]
|
||||
tune.pid.kf = 0.00012
|
||||
elif name == LatTunes.PID_H:
|
||||
tune.pid.kpV = [0.17]
|
||||
tune.pid.kiV = [0.03]
|
||||
tune.pid.kf = 0.00006
|
||||
elif name == LatTunes.PID_I:
|
||||
tune.pid.kpV = [0.15]
|
||||
tune.pid.kiV = [0.05]
|
||||
tune.pid.kf = 0.00004
|
||||
elif name == LatTunes.PID_J:
|
||||
tune.pid.kpV = [0.19]
|
||||
tune.pid.kiV = [0.02]
|
||||
tune.pid.kf = 0.00007818594
|
||||
elif name == LatTunes.PID_L:
|
||||
tune.pid.kpV = [0.3]
|
||||
tune.pid.kiV = [0.05]
|
||||
tune.pid.kf = 0.00006
|
||||
elif name == LatTunes.PID_M:
|
||||
tune.pid.kpV = [0.3]
|
||||
tune.pid.kiV = [0.05]
|
||||
tune.pid.kf = 0.00007
|
||||
elif name == LatTunes.PID_N:
|
||||
tune.pid.kpV = [0.35]
|
||||
tune.pid.kiV = [0.15]
|
||||
tune.pid.kf = 0.00007818594
|
||||
|
||||
# hyundai
|
||||
elif name == LatTunes.PID_HYUNDAI_A:
|
||||
tune.pid.kf = 0.00005
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.25], [0.05]]
|
||||
elif name == LatTunes.PID_HYUNDAI_B:
|
||||
tune.pid.kf = 0.00006
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.25], [0.05]]
|
||||
elif name == LatTunes.PID_HYUNDAI_C:
|
||||
tune.pid.kf = 0.00005
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.3], [0.05]]
|
||||
elif name == LatTunes.PID_HYUNDAI_D:
|
||||
tune.pid.kf = 0.00005
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[9., 22.], [9., 22.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.2, 0.35], [0.05, 0.09]]
|
||||
elif name == LatTunes.PID_HYUNDAI_E:
|
||||
tune.pid.kf = 0.
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.112], [0.004]]
|
||||
elif name == LatTunes.PID_HYUNDAI_F:
|
||||
tune.pid.kf = 0.00005
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.16], [0.01]]
|
||||
elif name == LatTunes.PID_HYUNDAI_G:
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0.], [0.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.16], [0.01]]
|
||||
|
||||
# vw
|
||||
elif name == LatTunes.PID_VW:
|
||||
tune.pid.kpBP = [0.]
|
||||
tune.pid.kiBP = [0.]
|
||||
tune.pid.kf = 0.00006
|
||||
tune.pid.kpV = [0.6]
|
||||
tune.pid.kiV = [0.2]
|
||||
|
||||
# subaru
|
||||
elif name == LatTunes.PID_SUBARU_A:
|
||||
tune.pid.kf = 0.00003
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0., 20.], [0., 20.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]]
|
||||
elif name == LatTunes.PID_SUBARU_B:
|
||||
tune.pid.kf = 0.00005
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0., 20.], [0., 20.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.2, 0.3], [0.02, 0.03]]
|
||||
elif name == LatTunes.PID_SUBARU_C:
|
||||
tune.pid.kf = 0.00005
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]]
|
||||
elif name == LatTunes.PID_SUBARU_D:
|
||||
tune.pid.kf = 0.000038
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
|
||||
elif name == LatTunes.PID_SUBARU_E:
|
||||
tune.pid.kf = 0.000039
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0., 10., 20.], [0., 10., 20.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.01, 0.05, 0.2], [0.003, 0.018, 0.025]]
|
||||
elif name == LatTunes.PID_SUBARU_F:
|
||||
tune.pid.kf = 0.00005
|
||||
tune.pid.kiBP, tune.pid.kpBP = [[0., 20.], [0., 20.]]
|
||||
tune.pid.kpV, tune.pid.kiV = [[0.1, 0.2], [0.01, 0.02]]
|
||||
|
||||
else:
|
||||
raise NotImplementedError('This PID tune does not exist')
|
||||
else:
|
||||
raise NotImplementedError('This lateral tune does not exist')
|
||||
@@ -12,6 +12,7 @@ class CarState(CarStateBase):
|
||||
super().__init__(CP)
|
||||
self.CCP = CarControllerParams(CP)
|
||||
self.button_states = {button.event_type: False for button in self.CCP.BUTTONS}
|
||||
self.esp_hold_confirmation = False
|
||||
|
||||
def create_button_events(self, pt_cp, buttons):
|
||||
button_events = []
|
||||
@@ -61,8 +62,12 @@ class CarState(CarStateBase):
|
||||
ret.gas = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] / 100.0
|
||||
ret.gasPressed = ret.gas > 0
|
||||
ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||
ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"])
|
||||
brake_pedal_pressed = bool(pt_cp.vl["Motor_14"]["MO_Fahrer_bremst"])
|
||||
brake_pressure_detected = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"])
|
||||
ret.brakePressed = brake_pedal_pressed or brake_pressure_detected
|
||||
ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) # FIXME: need to include an EPB check as well
|
||||
#dp
|
||||
ret.brakeLightsDEPRECATED = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck'] or ret.brakePressed or ret.parkingBrake)
|
||||
|
||||
# Update gear and/or clutch position data.
|
||||
if trans_type == TransmissionType.automatic:
|
||||
@@ -105,6 +110,7 @@ class CarState(CarStateBase):
|
||||
ret.stockAeb = bool(ext_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"])
|
||||
|
||||
# Update ACC radar status.
|
||||
self.acc_type = ext_cp.vl["ACC_06"]["ACC_Typ"]
|
||||
if pt_cp.vl["TSK_06"]["TSK_Status"] == 2:
|
||||
# ACC okay and enabled, but not currently engaged
|
||||
ret.cruiseState.available = True
|
||||
@@ -267,8 +273,9 @@ class CarState(CarStateBase):
|
||||
("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal including comfort blink interval
|
||||
("AB_Gurtschloss_FA", "Airbag_02"), # Seatbelt status, driver
|
||||
("AB_Gurtschloss_BF", "Airbag_02"), # Seatbelt status, passenger
|
||||
("ESP_Fahrer_bremst", "ESP_05"), # Brake pedal pressed
|
||||
("ESP_Bremsdruck", "ESP_05"), # Brake pressure applied
|
||||
("ESP_Fahrer_bremst", "ESP_05"), # Driver applied brake pressure over threshold
|
||||
("MO_Fahrer_bremst", "Motor_14"), # Brake pedal switch
|
||||
("ESP_Bremsdruck", "ESP_05"), # Brake pressure
|
||||
("MO_Fahrpedalrohwert_01", "Motor_20"), # Accelerator pedal value
|
||||
("EPS_Lenkmoment", "LH_EPS_03"), # Absolute driver torque input
|
||||
("EPS_VZ_Lenkmoment", "LH_EPS_03"), # Driver torque input sign
|
||||
@@ -289,6 +296,8 @@ class CarState(CarStateBase):
|
||||
("GRA_Tip_Stufe_2", "GRA_ACC_01"), # unknown related to stalk type
|
||||
("GRA_ButtonTypeInfo", "GRA_ACC_01"), # unknown related to stalk type
|
||||
("COUNTER", "GRA_ACC_01"), # GRA_ACC_01 CAN message counter
|
||||
#dp
|
||||
("ESP_Status_Bremsdruck", "ESP_05"), # Brakes applied
|
||||
]
|
||||
|
||||
checks = [
|
||||
@@ -303,6 +312,7 @@ class CarState(CarStateBase):
|
||||
("ESP_02", 50), # From J104 ABS/ESP controller
|
||||
("GRA_ACC_01", 33), # From J533 CAN gateway (via LIN from steering wheel controls)
|
||||
("Gateway_72", 10), # From J533 CAN gateway (aggregated data)
|
||||
("Motor_14", 10), # From J623 Engine control module
|
||||
("Airbag_02", 5), # From J234 Airbag control module
|
||||
("Kombi_01", 2), # From J285 Instrument cluster
|
||||
("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active)
|
||||
@@ -317,7 +327,6 @@ class CarState(CarStateBase):
|
||||
elif CP.transmissionType == TransmissionType.manual:
|
||||
signals += [("MO_Kuppl_schalter", "Motor_14"), # Clutch switch
|
||||
("BCM1_Rueckfahrlicht_Schalter", "Gateway_72")] # Reverse light from BCM
|
||||
checks.append(("Motor_14", 10)) # From J623 Engine control module
|
||||
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
# Radars are here on CANBUS.pt
|
||||
@@ -481,11 +490,13 @@ class MqbExtraSignals:
|
||||
# Additional signal and message lists for optional or bus-portable controllers
|
||||
fwd_radar_signals = [
|
||||
("ACC_Wunschgeschw_02", "ACC_02"), # ACC set speed
|
||||
("ACC_Typ", "ACC_06"), # Basic vs F2S vs SNG
|
||||
("AWV2_Freigabe", "ACC_10"), # FCW brake jerk release
|
||||
("ANB_Teilbremsung_Freigabe", "ACC_10"), # AEB partial braking release
|
||||
("ANB_Zielbremsung_Freigabe", "ACC_10"), # AEB target braking release
|
||||
]
|
||||
fwd_radar_checks = [
|
||||
("ACC_06", 50), # From J428 ACC radar control module
|
||||
("ACC_10", 50), # From J428 ACC radar control module
|
||||
("ACC_02", 17), # From J428 ACC radar control module
|
||||
]
|
||||
|
||||
@@ -27,19 +27,20 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.carName = "volkswagen"
|
||||
ret.radarOffCan = True
|
||||
|
||||
use_off_car_defaults = len(fingerprint[0]) == 0 # Pick sensible carParams during offline doc generation/CI jobs
|
||||
|
||||
if candidate in PQ_CARS:
|
||||
# Set global PQ35/PQ46/NMS parameters
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagenPq)]
|
||||
ret.enableBsm = 0x3BA in fingerprint[0] # SWA_1
|
||||
|
||||
if 0x440 in fingerprint[0]: # Getriebe_1
|
||||
if 0x440 in fingerprint[0] or use_off_car_defaults: # Getriebe_1
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
else:
|
||||
ret.transmissionType = TransmissionType.manual
|
||||
|
||||
if any(msg in fingerprint[1] for msg in (0x1A0, 0xC2)): # Bremse_1, Lenkwinkel_1
|
||||
ret.networkLocation = NetworkLocation.gateway
|
||||
ret.experimentalLongitudinalAvailable = True
|
||||
else:
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
|
||||
@@ -56,7 +57,7 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)]
|
||||
ret.enableBsm = 0x30F in fingerprint[0] # SWA_01
|
||||
|
||||
if 0xAD in fingerprint[0]: # Getriebe_11
|
||||
if 0xAD in fingerprint[0] or use_off_car_defaults: # Getriebe_11
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
elif 0x187 in fingerprint[0]: # EV_Gearshift
|
||||
ret.transmissionType = TransmissionType.direct
|
||||
@@ -82,7 +83,8 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
# Global longitudinal tuning defaults, can be overridden per-vehicle
|
||||
|
||||
if experimental_long and candidate in PQ_CARS:
|
||||
ret.experimentalLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or use_off_car_defaults
|
||||
if experimental_long:
|
||||
# Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required.
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL
|
||||
@@ -90,7 +92,11 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.minEnableSpeed = 4.5
|
||||
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.5 # s
|
||||
ret.stoppingControl = True
|
||||
ret.startingState = True
|
||||
ret.startAccel = 1.0
|
||||
ret.vEgoStarting = 1.0
|
||||
ret.vEgoStopping = 1.0
|
||||
ret.longitudinalTuning.kpV = [0.1]
|
||||
ret.longitudinalTuning.kiV = [0.0]
|
||||
|
||||
|
||||
@@ -36,3 +36,73 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, counter, cancel=Fa
|
||||
})
|
||||
|
||||
return packer.make_can_msg("GRA_ACC_01", bus, values)
|
||||
|
||||
|
||||
def acc_control_value(main_switch_on, acc_faulted, long_active):
|
||||
if acc_faulted:
|
||||
acc_control = 6
|
||||
elif long_active:
|
||||
acc_control = 3
|
||||
elif main_switch_on:
|
||||
acc_control = 2
|
||||
else:
|
||||
acc_control = 0
|
||||
|
||||
return acc_control
|
||||
|
||||
|
||||
def acc_hud_status_value(main_switch_on, acc_faulted, long_active):
|
||||
# TODO: happens to resemble the ACC control value for now, but extend this for init/gas override later
|
||||
return acc_control_value(main_switch_on, acc_faulted, long_active)
|
||||
|
||||
|
||||
def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, esp_hold):
|
||||
commands = []
|
||||
|
||||
acc_06_values = {
|
||||
"ACC_Typ": acc_type,
|
||||
"ACC_Status_ACC": acc_control,
|
||||
"ACC_StartStopp_Info": enabled,
|
||||
"ACC_Sollbeschleunigung_02": accel if enabled else 3.01,
|
||||
"ACC_zul_Regelabw_unten": 0.2, # TODO: dynamic adjustment of comfort-band
|
||||
"ACC_zul_Regelabw_oben": 0.2, # TODO: dynamic adjustment of comfort-band
|
||||
"ACC_neg_Sollbeschl_Grad_02": 4.0 if enabled else 0, # TODO: dynamic adjustment of jerk limits
|
||||
"ACC_pos_Sollbeschl_Grad_02": 4.0 if enabled else 0, # TODO: dynamic adjustment of jerk limits
|
||||
"ACC_Anfahren": starting,
|
||||
"ACC_Anhalten": stopping,
|
||||
}
|
||||
commands.append(packer.make_can_msg("ACC_06", bus, acc_06_values))
|
||||
|
||||
if starting:
|
||||
acc_hold_type = 4 # hold release / startup
|
||||
elif esp_hold:
|
||||
acc_hold_type = 3 # hold standby
|
||||
elif stopping:
|
||||
acc_hold_type = 1 # hold request
|
||||
else:
|
||||
acc_hold_type = 0
|
||||
|
||||
acc_07_values = {
|
||||
"ACC_Anhalteweg": 0.75 if stopping else 20.46, # Distance to stop (stopping coordinator handles terminal roll-out)
|
||||
"ACC_Freilauf_Info": 2 if enabled else 0,
|
||||
"ACC_Folgebeschl": 3.02, # Not using secondary controller accel unless and until we understand its impact
|
||||
"ACC_Sollbeschleunigung_02": accel if enabled else 3.01,
|
||||
"ACC_Anforderung_HMS": acc_hold_type,
|
||||
"ACC_Anfahren": starting,
|
||||
"ACC_Anhalten": stopping,
|
||||
}
|
||||
commands.append(packer.make_can_msg("ACC_07", bus, acc_07_values))
|
||||
|
||||
return commands
|
||||
|
||||
|
||||
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible):
|
||||
values = {
|
||||
"ACC_Status_Anzeige": acc_hud_status,
|
||||
"ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36,
|
||||
"ACC_Gesetzte_Zeitluecke": 3,
|
||||
"ACC_Display_Prio": 3,
|
||||
# TODO: ACC_Abstandsindex for lead car distance, must determine analog vs digital cluster for scaling
|
||||
}
|
||||
|
||||
return packer.make_can_msg("ACC_02", bus, values)
|
||||
|
||||
@@ -151,75 +151,76 @@ class Footnote(Enum):
|
||||
PASSAT = CarFootnote(
|
||||
"Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.",
|
||||
Column.MODEL)
|
||||
VW_HARNESS = CarFootnote(
|
||||
"Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma " +
|
||||
"store. Before ordering, remove the Lane Assist camera cover and check to see if the connector is black " +
|
||||
"(older design) or light brown (newer design). In the interim, if your car has a J533 connector CAN gateway " +
|
||||
"inside the dashboard, choose \"VW J533 Development\" from the vehicle drop-down for a suitable harness. " +
|
||||
"(Some newer models are also observed to not have a J533 connector.)",
|
||||
Column.MODEL)
|
||||
VW_VARIANT = CarFootnote(
|
||||
"Includes versions with extra rear cargo space (may be called Variant, Estate, SportWagen, Shooting Brake, etc.)",
|
||||
Column.MODEL)
|
||||
VW_EXP_LONG = CarFootnote (
|
||||
"Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness " +
|
||||
"are limited to using stock ACC.",
|
||||
Column.LONGITUDINAL)
|
||||
VW_MQB_A0 = CarFootnote(
|
||||
"Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot " +
|
||||
"in software, but doesn't yet have a harness available from the comma store.",
|
||||
Column.HARNESS)
|
||||
|
||||
|
||||
@dataclass
|
||||
class VWCarInfo(CarInfo):
|
||||
package: str = "Adaptive Cruise Control (ACC) & Lane Assist"
|
||||
harness: Enum = Harness.vw
|
||||
harness: Enum = Harness.j533
|
||||
|
||||
def init_make(self, CP: car.CarParams):
|
||||
self.footnotes.insert(0, Footnote.VW_EXP_LONG)
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
|
||||
CAR.ARTEON_MK1: [
|
||||
VWCarInfo("Volkswagen Arteon 2018-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"),
|
||||
VWCarInfo("Volkswagen Arteon R 2020-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"),
|
||||
VWCarInfo("Volkswagen Arteon eHybrid 2020-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"),
|
||||
VWCarInfo("Volkswagen CC 2018-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533, video_link="https://youtu.be/FAomFKPFlDA"),
|
||||
VWCarInfo("Volkswagen Arteon 2018-22", video_link="https://youtu.be/FAomFKPFlDA"),
|
||||
VWCarInfo("Volkswagen Arteon R 2020-22", video_link="https://youtu.be/FAomFKPFlDA"),
|
||||
VWCarInfo("Volkswagen Arteon eHybrid 2020-22", video_link="https://youtu.be/FAomFKPFlDA"),
|
||||
VWCarInfo("Volkswagen CC 2018-22", video_link="https://youtu.be/FAomFKPFlDA"),
|
||||
],
|
||||
CAR.ATLAS_MK1: [
|
||||
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),
|
||||
VWCarInfo("Volkswagen Teramont X 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Atlas 2018-23"),
|
||||
VWCarInfo("Volkswagen Atlas Cross Sport 2021-22"),
|
||||
VWCarInfo("Volkswagen Teramont 2018-22"),
|
||||
VWCarInfo("Volkswagen Teramont Cross Sport 2021-22"),
|
||||
VWCarInfo("Volkswagen Teramont X 2021-22"),
|
||||
],
|
||||
CAR.GOLF_MK7: [
|
||||
VWCarInfo("Volkswagen e-Golf 2014-20"),
|
||||
VWCarInfo("Volkswagen Golf 2015-20", footnotes=[Footnote.VW_VARIANT]),
|
||||
VWCarInfo("Volkswagen Golf 2015-20"),
|
||||
VWCarInfo("Volkswagen Golf Alltrack 2015-19"),
|
||||
VWCarInfo("Volkswagen Golf GTD 2015-20"),
|
||||
VWCarInfo("Volkswagen Golf GTE 2015-20"),
|
||||
VWCarInfo("Volkswagen Golf GTI 2015-21"),
|
||||
VWCarInfo("Volkswagen Golf R 2015-19", footnotes=[Footnote.VW_VARIANT]),
|
||||
VWCarInfo("Volkswagen Golf R 2015-19"),
|
||||
VWCarInfo("Volkswagen Golf SportsVan 2015-20"),
|
||||
],
|
||||
CAR.JETTA_MK7: [
|
||||
VWCarInfo("Volkswagen Jetta 2018-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Jetta GLI 2021-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Jetta 2018-22"),
|
||||
VWCarInfo("Volkswagen Jetta GLI 2021-22"),
|
||||
],
|
||||
CAR.PASSAT_MK8: [
|
||||
VWCarInfo("Volkswagen Passat 2015-22", footnotes=[Footnote.VW_HARNESS, Footnote.PASSAT, Footnote.VW_VARIANT], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Passat Alltrack 2015-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Passat GTE 2015-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Passat 2015-22", footnotes=[Footnote.PASSAT]),
|
||||
VWCarInfo("Volkswagen Passat Alltrack 2015-22"),
|
||||
VWCarInfo("Volkswagen Passat GTE 2015-22"),
|
||||
],
|
||||
CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22", harness=Harness.j533),
|
||||
CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22"),
|
||||
CAR.POLO_MK6: [
|
||||
VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_MQB_A0]),
|
||||
VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_MQB_A0]),
|
||||
],
|
||||
CAR.SHARAN_MK2: [
|
||||
VWCarInfo("Volkswagen Sharan 2018-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("SEAT Alhambra 2018-20", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Sharan 2018-22"),
|
||||
VWCarInfo("SEAT Alhambra 2018-20"),
|
||||
],
|
||||
CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022"),
|
||||
CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_MQB_A0]),
|
||||
CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22"),
|
||||
CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2017"),
|
||||
CAR.TRANSPORTER_T61: [
|
||||
VWCarInfo("Volkswagen Caravelle 2020", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen California 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Caravelle 2020"),
|
||||
VWCarInfo("Volkswagen California 2021"),
|
||||
],
|
||||
CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2021", footnotes=[Footnote.VW_MQB_A0]),
|
||||
CAR.AUDI_A3_MK3: [
|
||||
VWCarInfo("Audi A3 2014-19"),
|
||||
VWCarInfo("Audi A3 Sportback e-tron 2017-18"),
|
||||
@@ -227,14 +228,14 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
|
||||
VWCarInfo("Audi S3 2015-17"),
|
||||
],
|
||||
CAR.AUDI_Q2_MK1: VWCarInfo("Audi Q2 2018"),
|
||||
CAR.AUDI_Q3_MK2: VWCarInfo("Audi Q3 2020-21"),
|
||||
CAR.AUDI_Q3_MK2: VWCarInfo("Audi Q3 2019-23"),
|
||||
CAR.SEAT_ATECA_MK1: VWCarInfo("SEAT Ateca 2018"),
|
||||
CAR.SEAT_LEON_MK3: VWCarInfo("SEAT Leon 2014-20"),
|
||||
CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021", footnotes=[Footnote.KAMIQ]),
|
||||
CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-21", footnotes=[Footnote.VW_HARNESS]),
|
||||
CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ]),
|
||||
CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-21"),
|
||||
CAR.SKODA_KODIAQ_MK1: VWCarInfo("Škoda Kodiaq 2018-19"),
|
||||
CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020"),
|
||||
CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-18"),
|
||||
CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020", footnotes=[Footnote.VW_MQB_A0]),
|
||||
CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-22"),
|
||||
CAR.SKODA_OCTAVIA_MK3: [
|
||||
VWCarInfo("Škoda Octavia 2015, 2018-19"),
|
||||
VWCarInfo("Škoda Octavia RS 2016"),
|
||||
@@ -872,20 +873,24 @@ FW_VERSIONS = {
|
||||
CAR.AUDI_Q3_MK2: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xf1\x8705E906018N \xf1\x899970',
|
||||
b'\xf1\x8705L906022M \xf1\x890901',
|
||||
b'\xf1\x8783A906259 \xf1\x890001',
|
||||
b'\xf1\x8783A906259 \xf1\x890005',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x8709G927158CN\xf1\x893608',
|
||||
b'\xf1\x870GC300045D \xf1\x892802',
|
||||
b'\xf1\x870GC300046F \xf1\x892701',
|
||||
],
|
||||
(Ecu.srs, 0x715, None): [
|
||||
b'\xf1\x875Q0959655BF\xf1\x890403\xf1\x82\x1321211111211200311121232152219321422111',
|
||||
b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111224118A119321532111',
|
||||
b'\xf1\x875Q0959655CC\xf1\x890421\xf1\x82\x131111111111120031111237116A119321532111',
|
||||
],
|
||||
(Ecu.eps, 0x712, None): [
|
||||
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000300',
|
||||
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567G6000800',
|
||||
b'\xf1\x875QF909144B \xf1\x895582\xf1\x82\x0571G60533A1',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x757, None): [
|
||||
b'\xf1\x872Q0907572R \xf1\x890372',
|
||||
@@ -1061,6 +1066,7 @@ FW_VERSIONS = {
|
||||
},
|
||||
CAR.SKODA_SUPERB_MK3: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xf1\x8704L906026ET\xf1\x891343',
|
||||
b'\xf1\x8704L906026FP\xf1\x891196',
|
||||
b'\xf1\x8704L906026KB\xf1\x894071',
|
||||
b'\xf1\x8704L906026KD\xf1\x894798',
|
||||
@@ -1071,9 +1077,11 @@ FW_VERSIONS = {
|
||||
b'\xf1\x870CW300042H \xf1\x891601',
|
||||
b'\xf1\x870D9300011T \xf1\x894801',
|
||||
b'\xf1\x870D9300012 \xf1\x894940',
|
||||
b'\xf1\x870D9300041H \xf1\x894905',
|
||||
b'\xf1\x870GC300043 \xf1\x892301',
|
||||
],
|
||||
(Ecu.srs, 0x715, None): [
|
||||
b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\x12111200111121001121110012211292221111',
|
||||
b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\022111200111121001121118112231292221111',
|
||||
b'\xf1\x875Q0959655AK\xf1\x890130\xf1\x82\022111200111121001121110012211292221111',
|
||||
b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02331310031313100313131013141319331413100',
|
||||
|
||||
@@ -12,12 +12,11 @@ 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 is_tested_branch, get_short_branch
|
||||
from system.version import is_release_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.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.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature
|
||||
from selfdrive.controls.lib.latcontrol import LatControl
|
||||
from selfdrive.controls.lib.longcontrol import LongControl
|
||||
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
@@ -39,7 +38,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", "navd"
|
||||
IGNORE_PROCESSES = {"uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "statsd", "mapd", "navd", "gpxd",
|
||||
"logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad", "laikad"} | \
|
||||
{k for k, v in managed_processes.items() if not v.enabled}
|
||||
|
||||
@@ -50,7 +49,6 @@ Desire = log.LateralPlan.Desire
|
||||
LaneChangeState = log.LateralPlan.LaneChangeState
|
||||
LaneChangeDirection = log.LateralPlan.LaneChangeDirection
|
||||
EventName = car.CarEvent.EventName
|
||||
ButtonEvent = car.CarState.ButtonEvent
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
SafetyModel = car.CarParams.SafetyModel
|
||||
|
||||
@@ -150,7 +148,7 @@ class Controls:
|
||||
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
|
||||
self.CP.safetyConfigs = [safety_config]
|
||||
|
||||
if is_tested_branch():
|
||||
if is_release_branch():
|
||||
self.CP.experimentalLongitudinalAvailable = False
|
||||
|
||||
# Write CarParams for radard
|
||||
@@ -163,7 +161,7 @@ class Controls:
|
||||
if not self.CP.experimentalLongitudinalAvailable:
|
||||
self.params.remove("ExperimentalLongitudinalEnabled")
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
self.params.remove("EndToEndLong")
|
||||
self.params.remove("ExperimentalMode")
|
||||
|
||||
self.CC = car.CarControl.new_message()
|
||||
self.CS_prev = car.CarState.new_message()
|
||||
@@ -174,11 +172,7 @@ class Controls:
|
||||
self.VM = VehicleModel(self.CP)
|
||||
|
||||
self.LaC: LatControl
|
||||
if self.params.get_bool("dp_lateral_torque") and self.CP.lateralTuning.which() == 'torque':
|
||||
self.LaC = LatControlTorque(self.CP, self.CI)
|
||||
elif self.params.get_bool("dp_lateral_lqr") and self.CP.lateralTuning.which() == 'lqr':
|
||||
self.LaC = LatControlLQR(self.CP, self.CI)
|
||||
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
self.LaC = LatControlAngle(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP, self.CI)
|
||||
@@ -186,6 +180,8 @@ class Controls:
|
||||
self.LaC = LatControlINDI(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'torque':
|
||||
self.LaC = LatControlTorque(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'lqr':
|
||||
self.LaC = LatControlLQR(self.CP, self.CI)
|
||||
|
||||
self.initialized = False
|
||||
self.state = State.disabled
|
||||
@@ -193,9 +189,6 @@ class Controls:
|
||||
self.active = False
|
||||
self.can_rcv_timeout = False
|
||||
self.soft_disable_timer = 0
|
||||
self.v_cruise_kph = V_CRUISE_INITIAL
|
||||
self.v_cruise_cluster_kph = V_CRUISE_INITIAL
|
||||
self.v_cruise_kph_last = 0
|
||||
self.mismatch_counter = 0
|
||||
self.cruise_mismatch_counter = 0
|
||||
self.can_rcv_timeout_counter = 0
|
||||
@@ -205,11 +198,11 @@ class Controls:
|
||||
self.events_prev = []
|
||||
self.current_alert_types = [ET.PERMANENT]
|
||||
self.logged_comm_issue = None
|
||||
self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0}
|
||||
self.last_actuators = car.CarControl.Actuators.new_message()
|
||||
self.steer_limited = False
|
||||
self.desired_curvature = 0.0
|
||||
self.desired_curvature_rate = 0.0
|
||||
self.v_cruise_helper = VCruiseHelper(self.CP)
|
||||
|
||||
# TODO: no longer necessary, aside from process replay
|
||||
self.sm['liveParameters'].valid = True
|
||||
@@ -239,7 +232,7 @@ class Controls:
|
||||
controls_state = Params().get("ReplayControlsState")
|
||||
if controls_state is not None:
|
||||
controls_state = log.ControlsState.from_bytes(controls_state)
|
||||
self.v_cruise_kph = controls_state.vCruise
|
||||
self.v_cruise_helper.v_cruise_kph = controls_state.vCruise
|
||||
|
||||
if any(ps.controlsAllowed for ps in self.sm['pandaStates']):
|
||||
self.state = State.enabled
|
||||
@@ -265,17 +258,17 @@ class Controls:
|
||||
|
||||
# Block resume if cruise never previously enabled
|
||||
resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
|
||||
if not self.CP.pcmCruise and self.v_cruise_kph == V_CRUISE_INITIAL and resume_pressed:
|
||||
if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed:
|
||||
self.events.add(EventName.resumeBlocked)
|
||||
|
||||
# Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
|
||||
if self.sm['dragonConf'].dpAtl == 0 and ((CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
|
||||
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill))):
|
||||
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
|
||||
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill))):
|
||||
self.events.add(EventName.pedalPressed)
|
||||
|
||||
if CS.gasPressed:
|
||||
self.events.add(EventName.pedalPressedPreEnable if self.disengage_on_accelerator else
|
||||
EventName.gasPressedOverride)
|
||||
self.events.add(EventName.gasPressedOverride)
|
||||
|
||||
if not self.CP.notCar and not self.dp_jetson:
|
||||
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
|
||||
@@ -501,20 +494,7 @@ class Controls:
|
||||
def state_transition(self, CS):
|
||||
"""Compute conditional state transitions and execute actions on state transitions"""
|
||||
|
||||
self.v_cruise_kph_last = self.v_cruise_kph
|
||||
|
||||
if CS.cruiseState.available:
|
||||
# if stock cruise is completely disabled, then we can use our own set speed logic
|
||||
if not self.CP.pcmCruise:
|
||||
self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.vEgo, CS.gasPressed, CS.buttonEvents,
|
||||
self.button_timers, self.enabled, self.is_metric)
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
else:
|
||||
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
|
||||
self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
|
||||
else:
|
||||
self.v_cruise_kph = V_CRUISE_INITIAL
|
||||
self.v_cruise_cluster_kph = V_CRUISE_INITIAL
|
||||
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric)
|
||||
|
||||
# decrement the soft disable timer at every step, as it's reset on
|
||||
# entrance in SOFT_DISABLING state
|
||||
@@ -592,9 +572,7 @@ class Controls:
|
||||
else:
|
||||
self.state = State.enabled
|
||||
self.current_alert_types.append(ET.ENABLE)
|
||||
if not self.CP.pcmCruise:
|
||||
self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
self.v_cruise_helper.initialize_v_cruise(CS)
|
||||
|
||||
# Check if openpilot is engaged and actuators are enabled
|
||||
self.enabled = self.state in ENABLED_STATES
|
||||
@@ -645,7 +623,7 @@ class Controls:
|
||||
|
||||
if not self.joystick_mode:
|
||||
# accel PID loop
|
||||
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
|
||||
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS)
|
||||
t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL
|
||||
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
|
||||
|
||||
@@ -709,16 +687,6 @@ class Controls:
|
||||
|
||||
return CC, lac_log
|
||||
|
||||
def update_button_timers(self, buttonEvents):
|
||||
# increment timer for buttons still pressed
|
||||
for k in self.button_timers:
|
||||
if self.button_timers[k] > 0:
|
||||
self.button_timers[k] += 1
|
||||
|
||||
for b in buttonEvents:
|
||||
if b.type.raw in self.button_timers:
|
||||
self.button_timers[b.type.raw] = 1 if b.pressed else 0
|
||||
|
||||
def publish_logs(self, CS, start_time, CC, lac_log):
|
||||
|
||||
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
|
||||
@@ -745,7 +713,7 @@ class Controls:
|
||||
CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
|
||||
|
||||
hudControl = CC.hudControl
|
||||
hudControl.setSpeed = float(self.v_cruise_cluster_kph * CV.KPH_TO_MS)
|
||||
hudControl.setSpeed = float(self.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS)
|
||||
hudControl.speedVisible = self.enabled
|
||||
hudControl.lanesVisible = self.enabled
|
||||
hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
|
||||
@@ -828,8 +796,8 @@ class Controls:
|
||||
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
|
||||
controlsState.longControlState = self.LoC.long_control_state
|
||||
controlsState.vPid = float(self.LoC.v_pid)
|
||||
controlsState.vCruise = float(self.v_cruise_kph)
|
||||
controlsState.vCruiseCluster = float(self.v_cruise_cluster_kph)
|
||||
controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph)
|
||||
controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
|
||||
controlsState.upAccelCmd = float(self.LoC.pid.p)
|
||||
controlsState.uiAccelCmd = float(self.LoC.pid.i)
|
||||
controlsState.ufAccelCmd = float(self.LoC.pid.f)
|
||||
@@ -837,6 +805,7 @@ class Controls:
|
||||
controlsState.startMonoTime = int(start_time * 1e9)
|
||||
controlsState.forceDecel = bool(force_decel)
|
||||
controlsState.canErrorCounter = self.can_rcv_timeout_counter
|
||||
controlsState.experimentalMode = (self.params.get_bool("ExperimentalMode") or self.sm['longitudinalPlan'].dpE2EIsBlended) and self.CP.openpilotLongitudinalControl
|
||||
|
||||
lat_tuning = self.CP.lateralTuning.which()
|
||||
if self.joystick_mode:
|
||||
@@ -912,7 +881,6 @@ class Controls:
|
||||
self.publish_logs(CS, start_time, CC, lac_log)
|
||||
self.prof.checkpoint("Sent")
|
||||
|
||||
self.update_button_timers(CS.buttonEvents)
|
||||
self.CS_prev = CS
|
||||
|
||||
def controlsd_thread(self):
|
||||
@@ -921,6 +889,7 @@ class Controls:
|
||||
self.rk.monitor_time()
|
||||
self.prof.display()
|
||||
|
||||
|
||||
def main(sm=None, pm=None, logcan=None):
|
||||
controls = Controls(sm, pm, logcan)
|
||||
controls.controlsd_thread()
|
||||
|
||||
@@ -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 = 15 * CV.MPH_TO_MS
|
||||
LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
|
||||
LANE_CHANGE_TIME_MAX = 10.
|
||||
|
||||
DESIRES = {
|
||||
|
||||
@@ -12,6 +12,7 @@ V_CRUISE_MAX = 145 # kph
|
||||
V_CRUISE_MIN = 8 # kph
|
||||
V_CRUISE_ENABLE_MIN = 40 # kph
|
||||
V_CRUISE_INITIAL = 255 # kph
|
||||
IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors
|
||||
|
||||
MIN_SPEED = 1.0
|
||||
LAT_MPC_N = 16
|
||||
@@ -22,6 +23,7 @@ CAR_ROTATION_RADIUS = 0.0
|
||||
# EU guidelines
|
||||
MAX_LATERAL_JERK = 5.0
|
||||
|
||||
ButtonEvent = car.CarState.ButtonEvent
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
CRUISE_LONG_PRESS = 50
|
||||
CRUISE_NEAREST_FUNC = {
|
||||
@@ -47,6 +49,109 @@ class MPC_COST_LAT:
|
||||
HEADING = 1.0
|
||||
STEER_RATE = 1.0
|
||||
|
||||
class VCruiseHelper:
|
||||
def __init__(self, CP):
|
||||
self.CP = CP
|
||||
self.v_cruise_kph = V_CRUISE_INITIAL
|
||||
self.v_cruise_cluster_kph = V_CRUISE_INITIAL
|
||||
self.v_cruise_kph_last = 0
|
||||
self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
|
||||
self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
|
||||
|
||||
@property
|
||||
def v_cruise_initialized(self):
|
||||
return self.v_cruise_kph != V_CRUISE_INITIAL
|
||||
|
||||
def update_v_cruise(self, CS, enabled, is_metric):
|
||||
self.v_cruise_kph_last = self.v_cruise_kph
|
||||
|
||||
if CS.cruiseState.available:
|
||||
if not self.CP.pcmCruise:
|
||||
# if stock cruise is completely disabled, then we can use our own set speed logic
|
||||
self._update_v_cruise_non_pcm(CS, enabled, is_metric)
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
self.update_button_timers(CS, enabled)
|
||||
else:
|
||||
self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
|
||||
self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
|
||||
else:
|
||||
self.v_cruise_kph = V_CRUISE_INITIAL
|
||||
self.v_cruise_cluster_kph = V_CRUISE_INITIAL
|
||||
|
||||
def _update_v_cruise_non_pcm(self, CS, enabled, is_metric):
|
||||
# handle button presses. TODO: this should be in state_control, but a decelCruise press
|
||||
# would have the effect of both enabling and changing speed is checked after the state transition
|
||||
if not enabled:
|
||||
return
|
||||
|
||||
long_press = False
|
||||
button_type = None
|
||||
|
||||
v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT
|
||||
|
||||
for b in CS.buttonEvents:
|
||||
if b.type.raw in self.button_timers and not b.pressed:
|
||||
if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS:
|
||||
return # end long press
|
||||
button_type = b.type.raw
|
||||
break
|
||||
else:
|
||||
for k in self.button_timers.keys():
|
||||
if self.button_timers[k] and self.button_timers[k] % CRUISE_LONG_PRESS == 0:
|
||||
button_type = k
|
||||
long_press = True
|
||||
break
|
||||
|
||||
if button_type is None:
|
||||
return
|
||||
|
||||
# Don't adjust speed when pressing resume to exit standstill
|
||||
cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill
|
||||
if button_type == ButtonType.accelCruise and cruise_standstill:
|
||||
return
|
||||
|
||||
# Don't adjust speed if we've enabled since the button was depressed (some ports enable on rising edge)
|
||||
if not self.button_change_states[button_type]["enabled"]:
|
||||
return
|
||||
|
||||
v_cruise_delta = v_cruise_delta * (5 if long_press else 1)
|
||||
if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval
|
||||
self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta
|
||||
else:
|
||||
self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
|
||||
|
||||
# If set is pressed while overriding, clip cruise speed to minimum of vEgo
|
||||
if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
|
||||
self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH)
|
||||
|
||||
self.v_cruise_kph = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
|
||||
|
||||
def update_button_timers(self, CS, enabled):
|
||||
# increment timer for buttons still pressed
|
||||
for k in self.button_timers:
|
||||
if self.button_timers[k] > 0:
|
||||
self.button_timers[k] += 1
|
||||
|
||||
for b in CS.buttonEvents:
|
||||
if b.type.raw in self.button_timers:
|
||||
# Start/end timer and store current state on change of button pressed
|
||||
self.button_timers[b.type.raw] = 1 if b.pressed else 0
|
||||
self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
|
||||
|
||||
def initialize_v_cruise(self, CS):
|
||||
# initializing is handled by the PCM
|
||||
if self.CP.pcmCruise:
|
||||
return
|
||||
|
||||
# 250kph or above probably means we never had a set speed
|
||||
if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250:
|
||||
self.v_cruise_kph = self.v_cruise_kph_last
|
||||
else:
|
||||
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX)))
|
||||
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
|
||||
|
||||
def apply_deadzone(error, deadzone):
|
||||
if error > deadzone:
|
||||
error -= deadzone
|
||||
@@ -61,56 +166,6 @@ def rate_limit(new_value, last_value, dw_step, up_step):
|
||||
return clip(new_value, last_value + dw_step, last_value + up_step)
|
||||
|
||||
|
||||
def update_v_cruise(v_cruise_kph, v_ego, gas_pressed, buttonEvents, button_timers, enabled, metric):
|
||||
# handle button presses. TODO: this should be in state_control, but a decelCruise press
|
||||
# would have the effect of both enabling and changing speed is checked after the state transition
|
||||
if not enabled:
|
||||
return v_cruise_kph
|
||||
|
||||
long_press = False
|
||||
button_type = None
|
||||
|
||||
# should be CV.MPH_TO_KPH, but this causes rounding errors
|
||||
v_cruise_delta = 1. if metric else 1.6
|
||||
|
||||
for b in buttonEvents:
|
||||
if b.type.raw in button_timers and not b.pressed:
|
||||
if button_timers[b.type.raw] > CRUISE_LONG_PRESS:
|
||||
return v_cruise_kph # end long press
|
||||
button_type = b.type.raw
|
||||
break
|
||||
else:
|
||||
for k in button_timers.keys():
|
||||
if button_timers[k] and button_timers[k] % CRUISE_LONG_PRESS == 0:
|
||||
button_type = k
|
||||
long_press = True
|
||||
break
|
||||
|
||||
if button_type:
|
||||
v_cruise_delta = v_cruise_delta * (5 if long_press else 1)
|
||||
if long_press and v_cruise_kph % v_cruise_delta != 0: # partial interval
|
||||
v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](v_cruise_kph / v_cruise_delta) * v_cruise_delta
|
||||
else:
|
||||
v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
|
||||
|
||||
# If set is pressed while overriding, clip cruise speed to minimum of vEgo
|
||||
if gas_pressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
|
||||
v_cruise_kph = max(v_cruise_kph, v_ego * CV.MS_TO_KPH)
|
||||
|
||||
v_cruise_kph = clip(round(v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
|
||||
|
||||
return v_cruise_kph
|
||||
|
||||
|
||||
def initialize_v_cruise(v_ego, buttonEvents, v_cruise_last):
|
||||
for b in buttonEvents:
|
||||
# 250kph or above probably means we never had a set speed
|
||||
if b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) and v_cruise_last < 250:
|
||||
return v_cruise_last
|
||||
|
||||
return int(round(clip(v_ego * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN, V_CRUISE_MAX)))
|
||||
|
||||
|
||||
def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates, dp_lat_version):
|
||||
if dp_lat_version == 1: # 0813
|
||||
return get_0813_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates)
|
||||
|
||||
@@ -255,10 +255,9 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag
|
||||
|
||||
|
||||
def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
|
||||
gps_integrated = sm['peripheralState'].pandaType in (log.PandaState.PandaType.uno, log.PandaState.PandaType.dos)
|
||||
return Alert(
|
||||
_("Poor GPS reception"),
|
||||
_("Hardware malfunctioning if sky is visible") if gps_integrated else _("Check GPS antenna placement"),
|
||||
_("Hardware malfunctioning if sky is visible"),
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=300.)
|
||||
|
||||
@@ -516,7 +515,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
|
||||
EventName.resumeRequired: {
|
||||
ET.WARNING: Alert(
|
||||
_("STOPPED"),
|
||||
_("Press Resume to Go"),
|
||||
_("Press Resume to Exit Standstill"),
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
|
||||
},
|
||||
@@ -623,6 +622,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
|
||||
|
||||
EventName.buttonCancel: {
|
||||
ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
|
||||
ET.NO_ENTRY: NoEntryAlert("Cancel Pressed"),
|
||||
},
|
||||
|
||||
EventName.brakeHold: {
|
||||
@@ -912,7 +912,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
|
||||
ET.PERMANENT: Alert(
|
||||
_("Reverse\nGear"),
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.full,
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
|
||||
# ET.USER_DISABLE: ImmediateDisableAlert(_("Reverse Gear")),
|
||||
ET.NO_ENTRY: NoEntryAlert(_("Reverse Gear")),
|
||||
|
||||
@@ -17,7 +17,8 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
||||
# friction in the steering wheel that needs to be overcome to
|
||||
# move it at all, this is compensated for too.
|
||||
|
||||
LOW_SPEED_FACTOR = 200
|
||||
LOW_SPEED_X = [0, 10, 20, 30]
|
||||
LOW_SPEED_Y = [15, 13, 10, 5]
|
||||
|
||||
|
||||
class LatControlTorque(LatControl):
|
||||
@@ -57,8 +58,9 @@ class LatControlTorque(LatControl):
|
||||
actual_lateral_accel = actual_curvature * CS.vEgo ** 2
|
||||
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
|
||||
|
||||
setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
|
||||
measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
|
||||
low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
|
||||
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
|
||||
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
|
||||
error = setpoint - measurement
|
||||
gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
|
||||
pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error,
|
||||
|
||||
@@ -10,12 +10,14 @@ LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
|
||||
v_target_1sec, brake_pressed, cruise_standstill):
|
||||
# Ignore cruise standstill if car has a gas interceptor
|
||||
cruise_standstill = cruise_standstill and not CP.enableGasInterceptor
|
||||
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))
|
||||
(brake_pressed or cruise_standstill))
|
||||
stopping_condition = planned_stop or stay_stopped
|
||||
|
||||
starting_condition = (v_target_1sec > CP.vEgoStarting and
|
||||
@@ -103,6 +105,7 @@ class LongControl:
|
||||
|
||||
elif self.long_control_state == LongCtrlState.stopping:
|
||||
if output_accel > self.CP.stopAccel:
|
||||
output_accel = min(output_accel, 0.0)
|
||||
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
|
||||
self.reset(CS.vEgo)
|
||||
|
||||
|
||||
@@ -36,7 +36,7 @@ A_EGO_COST = 0.
|
||||
J_EGO_COST = 5.0
|
||||
A_CHANGE_COST = 200.
|
||||
DANGER_ZONE_COST = 100.
|
||||
CRASH_DISTANCE = .5
|
||||
CRASH_DISTANCE = .25
|
||||
LEAD_DANGER_FACTOR = 0.75
|
||||
LIMIT_COST = 1e6
|
||||
ACADOS_SOLVER_TYPE = 'SQP_RTI'
|
||||
@@ -49,6 +49,7 @@ MAX_T = 10.0
|
||||
T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)]
|
||||
|
||||
T_IDXS = np.array(T_IDXS_LST)
|
||||
FCW_IDXS = T_IDXS < 5.0
|
||||
T_DIFFS = np.diff(T_IDXS, prepend=[0.])
|
||||
MIN_ACCEL = -3.5
|
||||
MAX_ACCEL = 2.0
|
||||
@@ -56,7 +57,7 @@ T_FOLLOW = 1.45
|
||||
COMFORT_BRAKE = 2.5
|
||||
STOP_DISTANCE = 5.5
|
||||
|
||||
def get_stopped_equivalence_factor(v_lead, v_ego, t_follow=T_FOLLOW):
|
||||
def get_stopped_equivalence_factor(v_lead, v_ego):
|
||||
# KRKeegan this offset rapidly decreases the following distance when the lead pulls
|
||||
# away, resulting in an early demand for acceleration.
|
||||
v_diff_offset = 0
|
||||
@@ -71,7 +72,7 @@ def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW):
|
||||
return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
|
||||
|
||||
def desired_follow_distance(v_ego, v_lead, t_follow=T_FOLLOW):
|
||||
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead, v_ego, t_follow)
|
||||
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead, v_ego)
|
||||
|
||||
|
||||
def gen_long_model():
|
||||
@@ -260,27 +261,6 @@ class LongitudinalMpc:
|
||||
for i in range(N):
|
||||
self.solver.cost_set(i, 'Zl', Zl)
|
||||
|
||||
def get_cost_multipliers(self, v_lead0, v_lead1):
|
||||
v_ego = self.x0[1]
|
||||
v_ego_bps = [0, 10]
|
||||
TFs = [1.0, 1.25, T_FOLLOW, 1.8]
|
||||
# KRKeegan adjustments to costs for different TFs
|
||||
# these were calculated using the test_longitudial.py deceleration tests
|
||||
a_change_tf = interp(self.desired_TF, TFs, [.1, .8, 1., 1.1])
|
||||
j_ego_tf = interp(self.desired_TF, TFs, [.6, .8, 1., 1.1])
|
||||
d_zone_tf = interp(self.desired_TF, TFs, [1.6, 1.3, 1., 1.])
|
||||
# KRKeegan adjustments to improve sluggish acceleration
|
||||
# do not apply to deceleration
|
||||
j_ego_v_ego = 1
|
||||
a_change_v_ego = 1
|
||||
if (v_lead0 - v_ego >= 0) and (v_lead1 - v_ego >= 0):
|
||||
j_ego_v_ego = interp(v_ego, v_ego_bps, [.05, 1.])
|
||||
a_change_v_ego = interp(v_ego, v_ego_bps, [.05, 1.])
|
||||
# Select the appropriate min/max of the options
|
||||
j_ego = min(j_ego_tf, j_ego_v_ego)
|
||||
a_change = min(a_change_tf, a_change_v_ego)
|
||||
return a_change, j_ego, d_zone_tf
|
||||
|
||||
def set_weights(self, prev_accel_constraint=True, v_lead0=0, v_lead1=0):
|
||||
cost_multipliers = self.get_cost_multipliers(v_lead0, v_lead1)
|
||||
if self.mode == 'acc':
|
||||
@@ -335,8 +315,10 @@ class LongitudinalMpc:
|
||||
return lead_xv
|
||||
|
||||
def set_accel_limits(self, min_a, max_a):
|
||||
# TODO this sets a max accel limit, but the minimum limit is only for cruise decel
|
||||
# needs refactor
|
||||
self.cruise_min_a = min_a
|
||||
self.cruise_max_a = max_a
|
||||
self.max_a = max_a
|
||||
|
||||
def update(self, carstate, radarstate, v_cruise, x, v, a, j, prev_accel_constraint, desired_tf=T_FOLLOW):
|
||||
v_ego = self.x0[1]
|
||||
@@ -351,19 +333,20 @@ class LongitudinalMpc:
|
||||
# To estimate a safe distance from a moving lead, we calculate how much stopping
|
||||
# distance that lead needs as a minimum. We can add that to the current distance
|
||||
# and then treat that as a stopped car/obstacle at this new distance.
|
||||
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.x_sol[:,1], self.desired_TF)
|
||||
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.x_sol[:,1], self.desired_TF)
|
||||
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.x_sol[:,1])
|
||||
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.x_sol[:,1])
|
||||
|
||||
self.params[:,0] = MIN_ACCEL
|
||||
self.params[:,1] = self.max_a
|
||||
|
||||
# Update in ACC mode or ACC/e2e blend
|
||||
if self.mode == 'acc':
|
||||
self.params[:,0] = MIN_ACCEL
|
||||
self.params[:,1] = self.cruise_max_a
|
||||
self.params[:,5] = LEAD_DANGER_FACTOR
|
||||
|
||||
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
|
||||
# when the leads are no factor.
|
||||
v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
|
||||
v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05)
|
||||
v_upper = v_ego + (T_IDXS * self.max_a * 1.05)
|
||||
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
|
||||
v_lower,
|
||||
v_upper)
|
||||
@@ -375,9 +358,6 @@ class LongitudinalMpc:
|
||||
x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0
|
||||
|
||||
elif self.mode == 'blended':
|
||||
self.params[:,0] = MIN_ACCEL
|
||||
self.params[:,1] = MAX_ACCEL
|
||||
|
||||
self.params[:,5] = 1.0
|
||||
|
||||
x_obstacles = np.column_stack([lead_0_obstacle,
|
||||
@@ -389,7 +369,7 @@ class LongitudinalMpc:
|
||||
x_and_cruise = np.column_stack([x, cruise_target])
|
||||
x = np.min(x_and_cruise, axis=1)
|
||||
|
||||
self.source = 'e2e' if x_and_cruise[0,0] < x_and_cruise[0,1] else 'cruise'
|
||||
self.source = 'e2e' if x_and_cruise[1,0] < x_and_cruise[1,1] else 'cruise'
|
||||
|
||||
else:
|
||||
raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update')
|
||||
@@ -407,8 +387,8 @@ class LongitudinalMpc:
|
||||
self.params[:,4] = self.desired_TF
|
||||
|
||||
self.run()
|
||||
if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
|
||||
radarstate.leadOne.modelProb > 0.9):
|
||||
if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
|
||||
radarstate.leadOne.modelProb > 0.9):
|
||||
self.crash_cnt += 1
|
||||
else:
|
||||
self.crash_cnt = 0
|
||||
@@ -419,7 +399,7 @@ class LongitudinalMpc:
|
||||
if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], self.desired_TF))- self.x_sol[:,0] < 0.0):
|
||||
self.source = 'lead0'
|
||||
if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], self.desired_TF))- self.x_sol[:,0] < 0.0) and \
|
||||
(lead_1_obstacle[0] - lead_0_obstacle[0]):
|
||||
(lead_1_obstacle[0] - lead_0_obstacle[0]):
|
||||
self.source = 'lead1'
|
||||
|
||||
def run(self):
|
||||
@@ -462,6 +442,26 @@ class LongitudinalMpc:
|
||||
# reset = 1
|
||||
# print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(sec_since_boot() - t0):.2e} qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}")
|
||||
|
||||
def get_cost_multipliers(self, v_lead0, v_lead1):
|
||||
v_ego = self.x0[1]
|
||||
v_ego_bps = [0, 10]
|
||||
TFs = [1.0, 1.25, T_FOLLOW, 1.8]
|
||||
# KRKeegan adjustments to costs for different TFs
|
||||
# these were calculated using the test_longitudial.py deceleration tests
|
||||
a_change_tf = interp(self.desired_TF, TFs, [.1, .8, 1., 1.1])
|
||||
j_ego_tf = interp(self.desired_TF, TFs, [.6, .8, 1., 1.1])
|
||||
d_zone_tf = interp(self.desired_TF, TFs, [1.6, 1.3, 1., 1.])
|
||||
# KRKeegan adjustments to improve sluggish acceleration
|
||||
# do not apply to deceleration
|
||||
j_ego_v_ego = 1
|
||||
a_change_v_ego = 1
|
||||
if (v_lead0 - v_ego >= 0) and (v_lead1 - v_ego >= 0):
|
||||
j_ego_v_ego = interp(v_ego, v_ego_bps, [.05, 1.])
|
||||
a_change_v_ego = interp(v_ego, v_ego_bps, [.05, 1.])
|
||||
# Select the appropriate min/max of the options
|
||||
j_ego = min(j_ego_tf, j_ego_v_ego)
|
||||
a_change = min(a_change_tf, a_change_v_ego)
|
||||
return a_change, j_ego, d_zone_tf
|
||||
|
||||
if __name__ == "__main__":
|
||||
ocp = gen_long_ocp()
|
||||
|
||||
@@ -10,7 +10,7 @@ from common.params import Params
|
||||
from common.realtime import DT_MDL
|
||||
from selfdrive.modeld.constants import T_IDXS
|
||||
from selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, T_FOLLOW
|
||||
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, MIN_ACCEL, MAX_ACCEL, T_FOLLOW
|
||||
from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
|
||||
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
|
||||
from system.swaglog import cloudlog
|
||||
@@ -47,7 +47,7 @@ _DP_CRUISE_MIN_V_SPORT = [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1
|
||||
_DP_CRUISE_MIN_BP = [0., 0.07, 6., 8., 11., 15., 20., 25., 30., 55.]
|
||||
|
||||
_DP_CRUISE_MAX_V = [3.5, 1.7, 1.31, 0.95, 0.77, 0.67, 0.55, 0.47, 0.31, 0.13]
|
||||
_DP_CRUISE_MAX_V_ECO = [2.7, 1.4, 1.2, 0.7, 0.48, 0.35, 0.25, 0.15, 0.12, 0.06]
|
||||
_DP_CRUISE_MAX_V_ECO = [2.5, 1.3, 1.2, 0.7, 0.48, 0.35, 0.25, 0.15, 0.12, 0.06]
|
||||
_DP_CRUISE_MAX_V_SPORT = [3.5, 3.5, 2.5, 1.5, 2.0, 2.0, 2.0, 1.5, 1.0, 0.5]
|
||||
_DP_CRUISE_MAX_BP = [0., 3, 6., 8., 11., 15., 20., 25., 30., 55.]
|
||||
|
||||
@@ -111,6 +111,7 @@ class LongitudinalPlanner:
|
||||
|
||||
self.a_desired = init_a
|
||||
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
|
||||
self.v_model_error = 0.0
|
||||
|
||||
self.v_desired_trajectory = np.zeros(CONTROL_N)
|
||||
self.a_desired_trajectory = np.zeros(CONTROL_N)
|
||||
@@ -127,13 +128,14 @@ class LongitudinalPlanner:
|
||||
self.speed_limit_controller = SpeedLimitController()
|
||||
self.events = Events()
|
||||
self.turn_speed_controller = TurnSpeedController()
|
||||
self.dp_e2e_adapt_ap = False
|
||||
|
||||
def read_param(self):
|
||||
e2e = self.params.get_bool('EndToEndLong') and self.CP.openpilotLongitudinalControl
|
||||
e2e = self.params.get_bool('ExperimentalMode') and self.CP.openpilotLongitudinalControl
|
||||
self.mpc.mode = 'blended' if e2e else 'acc'
|
||||
|
||||
# dp - conditional e2e
|
||||
def conditional_e2e(self, standstill, within_speed_condition, e2e_lead, lead_rel_speed):
|
||||
def conditional_e2e(self, standstill, within_speed_condition, e2e_lead, within_speed_condition_lead):
|
||||
reset_state = False
|
||||
|
||||
# lead counter
|
||||
@@ -164,9 +166,9 @@ class LongitudinalPlanner:
|
||||
self.dp_e2e_sng = False
|
||||
dp_e2e_mode = 'blended'
|
||||
else:
|
||||
# lead is driving below 30 km/h
|
||||
# lead is driving below x km/h
|
||||
if self.dp_e2e_has_lead:
|
||||
if lead_rel_speed <= 8.3:
|
||||
if within_speed_condition_lead:
|
||||
dp_e2e_mode = 'blended'
|
||||
else:
|
||||
# within speed condition and does not have a lead, use e2e
|
||||
@@ -183,12 +185,13 @@ class LongitudinalPlanner:
|
||||
|
||||
return reset_state
|
||||
|
||||
def parse_model(self, model_msg):
|
||||
@staticmethod
|
||||
def parse_model(model_msg, model_error):
|
||||
if (len(model_msg.position.x) == 33 and
|
||||
len(model_msg.velocity.x) == 33 and
|
||||
len(model_msg.acceleration.x) == 33):
|
||||
x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x)
|
||||
v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x)
|
||||
x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
|
||||
v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) - model_error
|
||||
a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
|
||||
j = np.zeros(len(T_IDXS_MPC))
|
||||
else:
|
||||
@@ -200,7 +203,9 @@ class LongitudinalPlanner:
|
||||
|
||||
def get_df(self, v_ego):
|
||||
desired_tf = T_FOLLOW
|
||||
if self.dp_following_profile_ctrl and self.mpc.mode == 'acc':
|
||||
if not self.dp_e2e_adapt_ap and self.mpc.mode == 'blended':
|
||||
return desired_tf
|
||||
if self.dp_following_profile_ctrl:
|
||||
if self.dp_following_profile == 0:
|
||||
# At slow speeds more time, decrease time up to 60mph
|
||||
# in kph ~= 0 20 40 50 70 80 90 150
|
||||
@@ -231,10 +236,11 @@ class LongitudinalPlanner:
|
||||
dp_reset_state = False
|
||||
|
||||
if sm['dragonConf'].dpE2EConditional:
|
||||
self.dp_e2e_adapt_ap = sm['dragonConf'].dpE2EConditionalAdaptAp
|
||||
e2e_lead = sm['radarState'].leadOne.status and sm['radarState'].leadOne.dRel <= _DP_E2E_LEAD_DIST
|
||||
within_speed_condition = sm['controlsState'].vCruise <= sm['dragonConf'].dpE2EConditionalAtSpeed
|
||||
lead_rel_speed = sm['radarState'].leadOne.vRel + sm['carState'].vEgo
|
||||
if self.conditional_e2e(sm['carState'].standstill, within_speed_condition, e2e_lead, lead_rel_speed):
|
||||
within_speed_condition_lead = (sm['radarState'].leadOne.vRel + sm['carState'].vEgo)*3.6 <= sm['dragonConf'].dpE2EConditionalAtSpeedLead
|
||||
if self.conditional_e2e(sm['carState'].standstill, within_speed_condition, e2e_lead, within_speed_condition_lead):
|
||||
dp_reset_state = True
|
||||
else:
|
||||
if self.param_read_counter % 50 == 0 and read:
|
||||
@@ -255,11 +261,19 @@ class LongitudinalPlanner:
|
||||
# No change cost when user is controlling the speed, or when standstill
|
||||
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
|
||||
|
||||
if not self.dp_accel_profile_ctrl:
|
||||
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
|
||||
if self.mpc.mode == 'acc':
|
||||
if self.dp_accel_profile_ctrl:
|
||||
accel_limits = dp_calc_cruise_accel_limits(v_ego, self.dp_accel_profile)
|
||||
else:
|
||||
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
|
||||
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
|
||||
else:
|
||||
accel_limits = dp_calc_cruise_accel_limits(v_ego, self.dp_accel_profile)
|
||||
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
|
||||
if sm['dragonConf'].dpE2EConditional and sm['dragonConf'].dpE2EConditionalAdaptAp and self.dp_accel_profile_ctrl:
|
||||
_, accel_max = dp_calc_cruise_accel_limits(v_ego, self.dp_accel_profile)
|
||||
accel_limits = [MIN_ACCEL, accel_max]
|
||||
else:
|
||||
accel_limits = [MIN_ACCEL, MAX_ACCEL]
|
||||
accel_limits_turns = [MIN_ACCEL, MAX_ACCEL]
|
||||
|
||||
if reset_state or dp_reset_state:
|
||||
self.v_desired_filter.x = v_ego
|
||||
@@ -268,6 +282,9 @@ class LongitudinalPlanner:
|
||||
|
||||
# Prevent divergence, smooth in current v_ego
|
||||
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
|
||||
# Compute model v_ego error
|
||||
if len(sm['modelV2'].temporalPose.trans):
|
||||
self.v_model_error = sm['modelV2'].temporalPose.trans[0] - v_ego
|
||||
|
||||
# Get acceleration and active solutions for custom long mpc.
|
||||
self.cruise_source, a_min_sol, v_cruise_sol = self.cruise_solutions(not reset_state, self.v_desired_filter.x,
|
||||
@@ -277,14 +294,15 @@ class LongitudinalPlanner:
|
||||
# if required so, force a smooth deceleration
|
||||
accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
|
||||
accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
|
||||
|
||||
# clip limits, cannot init MPC outside of bounds
|
||||
accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05, a_min_sol)
|
||||
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
|
||||
|
||||
# dp - mpc.set_weights calls moved to mpc.update function because we need lead0 and lead1 data
|
||||
# self.mpc.set_weights(prev_accel_constraint)
|
||||
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
x, v, a, j = self.parse_model(sm['modelV2'])
|
||||
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
|
||||
self.mpc.update(sm['carState'], sm['radarState'], v_cruise_sol, x, v, a, j, prev_accel_constraint, self.get_df(v_ego))
|
||||
|
||||
self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
|
||||
@@ -292,8 +310,7 @@ class LongitudinalPlanner:
|
||||
self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
|
||||
|
||||
# TODO counter is only needed because radar is glitchy, remove once radar is gone
|
||||
# TODO write fcw in e2e_long mode
|
||||
self.fcw = self.mpc.mode == 'acc' and self.mpc.crash_cnt > 5
|
||||
self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
|
||||
if self.fcw:
|
||||
cloudlog.info("FCW triggered")
|
||||
|
||||
|
||||
@@ -151,7 +151,8 @@ class Cluster():
|
||||
|
||||
def potential_low_speed_lead(self, v_ego):
|
||||
# stop for stuff in front of you and low speed, even without model confirmation
|
||||
return abs(self.yRel) < 1.0 and (v_ego < v_ego_stationary) and self.dRel < 25
|
||||
# Radar points closer than 0.75, are almost always glitches on toyota radars
|
||||
return abs(self.yRel) < 1.0 and (v_ego < v_ego_stationary) and (0.75 < self.dRel < 25)
|
||||
|
||||
def is_potential_fcw(self, model_prob):
|
||||
return model_prob > .9
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 84 KiB |
@@ -30,6 +30,7 @@ import requests
|
||||
import math
|
||||
from common.basedir import BASEDIR
|
||||
from common.params import Params
|
||||
from common.i18n import supported_languages
|
||||
params = Params()
|
||||
|
||||
hostName = ""
|
||||
@@ -254,6 +255,17 @@ class OtisServ(BaseHTTPRequestHandler):
|
||||
return "121.3149803", "24.996256935"
|
||||
return "-117.1662042", "32.7207742"
|
||||
|
||||
def get_lang(self):
|
||||
lang = params.get("LanguageSetting", encoding='utf8')
|
||||
try:
|
||||
if lang is not None:
|
||||
lang = supported_languages[lang.strip()]
|
||||
else:
|
||||
lang = "en-US"
|
||||
except KeyError:
|
||||
lang = "en-US"
|
||||
return lang
|
||||
|
||||
def display_page_gmap_key(self):
|
||||
self.wfile.write(bytes(self.get_parsed_template("body", {"{{content}}": self.get_parsed_template("gmap/key_input")}), "utf-8"))
|
||||
|
||||
@@ -274,7 +286,7 @@ class OtisServ(BaseHTTPRequestHandler):
|
||||
self.wfile.write(bytes(self.get_parsed_template("body", {"{{content}}": content }), "utf-8"))
|
||||
|
||||
def display_page_gmap(self):
|
||||
self.wfile.write(bytes(self.get_parsed_template("gmap/index.html", {"{{gmap_key}}": self.get_gmap_key()}), "utf-8"))
|
||||
self.wfile.write(bytes(self.get_parsed_template("gmap/index.html", {"{{gmap_key}}": self.get_gmap_key(), "{{language}}": self.get_lang()}), "utf-8"))
|
||||
|
||||
def display_page_amap(self):
|
||||
self.wfile.write(bytes(self.get_parsed_template("amap/index.html", {"{{amap_key}}": self.get_amap_key(), "{{amap_key_2}}": self.get_amap_key_2()}), "utf-8"))
|
||||
|
||||
@@ -28,7 +28,7 @@ This is a service that broadcast dp config values to openpilot's messaging queue
|
||||
import cereal.messaging as messaging
|
||||
|
||||
from common.dp_conf import confs, get_struct_name, to_struct_val
|
||||
from common.params import Params
|
||||
from common.params import Params, put_bool_nonblocking
|
||||
import os
|
||||
params = Params()
|
||||
from common.dp_helpers import get_last_modified, LAST_MODIFIED_TIMER_SYSTEMD
|
||||
@@ -62,6 +62,7 @@ def confd_thread():
|
||||
dashcamd = Dashcamd()
|
||||
rk = Ratekeeper(HERTZ, print_delay_threshold=None) # Keeps rate at 2 hz
|
||||
uploader_thread = None
|
||||
dp_jetson = params.get_bool('dp_jetson')
|
||||
|
||||
while True:
|
||||
if uploader_thread is None:
|
||||
@@ -139,6 +140,8 @@ def confd_thread():
|
||||
===================================================
|
||||
'''
|
||||
msg = update_custom_logic(msg)
|
||||
if dp_jetson:
|
||||
msg.dragonConf.dpUiFace = False
|
||||
'''
|
||||
===================================================
|
||||
battery ctrl every 30 secs
|
||||
|
||||
@@ -32,8 +32,8 @@
|
||||
<div id="map"></div>
|
||||
<!-- Async script executes immediately and must be after any DOM elements used in callback. -->
|
||||
<script
|
||||
src="https://maps.googleapis.com/maps/api/js?key={{gmap_key}}&callback=initAutocomplete&libraries=places&v=weekly"
|
||||
src="https://maps.googleapis.com/maps/api/js?key={{gmap_key}}&callback=initAutocomplete&libraries=places&v=weekly&language={{language}}"
|
||||
async
|
||||
></script>
|
||||
</body>
|
||||
</html>
|
||||
</html>
|
||||
|
||||
@@ -166,10 +166,7 @@ class Calibrator:
|
||||
self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
|
||||
|
||||
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
|
||||
if self.wide_camera:
|
||||
angle_std_threshold = 4*MAX_VEL_ANGLE_STD
|
||||
else:
|
||||
angle_std_threshold = MAX_VEL_ANGLE_STD
|
||||
angle_std_threshold = MAX_VEL_ANGLE_STD
|
||||
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < angle_std_threshold) or
|
||||
(self.valid_blocks < INPUTS_NEEDED))
|
||||
if not (straight_and_fast and certain_if_calib):
|
||||
@@ -185,7 +182,6 @@ class Calibrator:
|
||||
new_wide_from_device_euler = np.array(wide_from_device_euler)
|
||||
else:
|
||||
new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
|
||||
|
||||
self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] +
|
||||
(BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
|
||||
self.wide_from_device_eulers[self.block_idx] = (self.idx*self.wide_from_device_eulers[self.block_idx] +
|
||||
|
||||
@@ -187,6 +187,7 @@ class Laikad:
|
||||
"gpsTimeOfWeek": tow,
|
||||
"positionECEF": measurement_msg(value=ecef_pos.tolist(), std=pos_std.tolist(), valid=kf_valid),
|
||||
"velocityECEF": measurement_msg(value=ecef_vel.tolist(), std=vel_std.tolist(), valid=kf_valid),
|
||||
# TODO std is incorrectly the dimension of the measurements and not 3D
|
||||
"positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t),
|
||||
"ubloxMonoTime": gnss_mono_time,
|
||||
"correctedMeasurements": meas_msgs
|
||||
|
||||
Binary file not shown.
@@ -32,7 +32,7 @@ public:
|
||||
void finite_check(double current_time = NAN);
|
||||
void time_check(double current_time = NAN);
|
||||
void update_reset_tracker();
|
||||
bool isGpsOK();
|
||||
bool is_gps_ok();
|
||||
void determine_gps_mode(double current_time);
|
||||
|
||||
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
|
||||
@@ -69,8 +69,9 @@ private:
|
||||
std::unique_ptr<LocalCoord> converter;
|
||||
|
||||
int64_t unix_timestamp_millis = 0;
|
||||
double last_gps_fix = 0;
|
||||
double reset_tracker = 0.0;
|
||||
bool device_fell = false;
|
||||
bool gps_mode = false;
|
||||
bool gps_valid = false;
|
||||
// bool ublox_available = true;
|
||||
};
|
||||
|
||||
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
|
||||
* *
|
||||
* This file is part of 'ekf' *
|
||||
******************************************************************************/
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_1840749225442711100) {
|
||||
out_1840749225442711100[0] = delta_x[0] + nom_x[0];
|
||||
out_1840749225442711100[1] = delta_x[1] + nom_x[1];
|
||||
out_1840749225442711100[2] = delta_x[2] + nom_x[2];
|
||||
out_1840749225442711100[3] = delta_x[3] + nom_x[3];
|
||||
out_1840749225442711100[4] = delta_x[4] + nom_x[4];
|
||||
out_1840749225442711100[5] = delta_x[5] + nom_x[5];
|
||||
out_1840749225442711100[6] = delta_x[6] + nom_x[6];
|
||||
out_1840749225442711100[7] = delta_x[7] + nom_x[7];
|
||||
out_1840749225442711100[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_7743502251243671447) {
|
||||
out_7743502251243671447[0] = delta_x[0] + nom_x[0];
|
||||
out_7743502251243671447[1] = delta_x[1] + nom_x[1];
|
||||
out_7743502251243671447[2] = delta_x[2] + nom_x[2];
|
||||
out_7743502251243671447[3] = delta_x[3] + nom_x[3];
|
||||
out_7743502251243671447[4] = delta_x[4] + nom_x[4];
|
||||
out_7743502251243671447[5] = delta_x[5] + nom_x[5];
|
||||
out_7743502251243671447[6] = delta_x[6] + nom_x[6];
|
||||
out_7743502251243671447[7] = delta_x[7] + nom_x[7];
|
||||
out_7743502251243671447[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197) {
|
||||
out_7887258947850824197[0] = -nom_x[0] + true_x[0];
|
||||
out_7887258947850824197[1] = -nom_x[1] + true_x[1];
|
||||
out_7887258947850824197[2] = -nom_x[2] + true_x[2];
|
||||
out_7887258947850824197[3] = -nom_x[3] + true_x[3];
|
||||
out_7887258947850824197[4] = -nom_x[4] + true_x[4];
|
||||
out_7887258947850824197[5] = -nom_x[5] + true_x[5];
|
||||
out_7887258947850824197[6] = -nom_x[6] + true_x[6];
|
||||
out_7887258947850824197[7] = -nom_x[7] + true_x[7];
|
||||
out_7887258947850824197[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8632432640030731669) {
|
||||
out_8632432640030731669[0] = -nom_x[0] + true_x[0];
|
||||
out_8632432640030731669[1] = -nom_x[1] + true_x[1];
|
||||
out_8632432640030731669[2] = -nom_x[2] + true_x[2];
|
||||
out_8632432640030731669[3] = -nom_x[3] + true_x[3];
|
||||
out_8632432640030731669[4] = -nom_x[4] + true_x[4];
|
||||
out_8632432640030731669[5] = -nom_x[5] + true_x[5];
|
||||
out_8632432640030731669[6] = -nom_x[6] + true_x[6];
|
||||
out_8632432640030731669[7] = -nom_x[7] + true_x[7];
|
||||
out_8632432640030731669[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_5849575757077882696) {
|
||||
out_5849575757077882696[0] = 1.0;
|
||||
out_5849575757077882696[1] = 0;
|
||||
out_5849575757077882696[2] = 0;
|
||||
out_5849575757077882696[3] = 0;
|
||||
out_5849575757077882696[4] = 0;
|
||||
out_5849575757077882696[5] = 0;
|
||||
out_5849575757077882696[6] = 0;
|
||||
out_5849575757077882696[7] = 0;
|
||||
out_5849575757077882696[8] = 0;
|
||||
out_5849575757077882696[9] = 0;
|
||||
out_5849575757077882696[10] = 1.0;
|
||||
out_5849575757077882696[11] = 0;
|
||||
out_5849575757077882696[12] = 0;
|
||||
out_5849575757077882696[13] = 0;
|
||||
out_5849575757077882696[14] = 0;
|
||||
out_5849575757077882696[15] = 0;
|
||||
out_5849575757077882696[16] = 0;
|
||||
out_5849575757077882696[17] = 0;
|
||||
out_5849575757077882696[18] = 0;
|
||||
out_5849575757077882696[19] = 0;
|
||||
out_5849575757077882696[20] = 1.0;
|
||||
out_5849575757077882696[21] = 0;
|
||||
out_5849575757077882696[22] = 0;
|
||||
out_5849575757077882696[23] = 0;
|
||||
out_5849575757077882696[24] = 0;
|
||||
out_5849575757077882696[25] = 0;
|
||||
out_5849575757077882696[26] = 0;
|
||||
out_5849575757077882696[27] = 0;
|
||||
out_5849575757077882696[28] = 0;
|
||||
out_5849575757077882696[29] = 0;
|
||||
out_5849575757077882696[30] = 1.0;
|
||||
out_5849575757077882696[31] = 0;
|
||||
out_5849575757077882696[32] = 0;
|
||||
out_5849575757077882696[33] = 0;
|
||||
out_5849575757077882696[34] = 0;
|
||||
out_5849575757077882696[35] = 0;
|
||||
out_5849575757077882696[36] = 0;
|
||||
out_5849575757077882696[37] = 0;
|
||||
out_5849575757077882696[38] = 0;
|
||||
out_5849575757077882696[39] = 0;
|
||||
out_5849575757077882696[40] = 1.0;
|
||||
out_5849575757077882696[41] = 0;
|
||||
out_5849575757077882696[42] = 0;
|
||||
out_5849575757077882696[43] = 0;
|
||||
out_5849575757077882696[44] = 0;
|
||||
out_5849575757077882696[45] = 0;
|
||||
out_5849575757077882696[46] = 0;
|
||||
out_5849575757077882696[47] = 0;
|
||||
out_5849575757077882696[48] = 0;
|
||||
out_5849575757077882696[49] = 0;
|
||||
out_5849575757077882696[50] = 1.0;
|
||||
out_5849575757077882696[51] = 0;
|
||||
out_5849575757077882696[52] = 0;
|
||||
out_5849575757077882696[53] = 0;
|
||||
out_5849575757077882696[54] = 0;
|
||||
out_5849575757077882696[55] = 0;
|
||||
out_5849575757077882696[56] = 0;
|
||||
out_5849575757077882696[57] = 0;
|
||||
out_5849575757077882696[58] = 0;
|
||||
out_5849575757077882696[59] = 0;
|
||||
out_5849575757077882696[60] = 1.0;
|
||||
out_5849575757077882696[61] = 0;
|
||||
out_5849575757077882696[62] = 0;
|
||||
out_5849575757077882696[63] = 0;
|
||||
out_5849575757077882696[64] = 0;
|
||||
out_5849575757077882696[65] = 0;
|
||||
out_5849575757077882696[66] = 0;
|
||||
out_5849575757077882696[67] = 0;
|
||||
out_5849575757077882696[68] = 0;
|
||||
out_5849575757077882696[69] = 0;
|
||||
out_5849575757077882696[70] = 1.0;
|
||||
out_5849575757077882696[71] = 0;
|
||||
out_5849575757077882696[72] = 0;
|
||||
out_5849575757077882696[73] = 0;
|
||||
out_5849575757077882696[74] = 0;
|
||||
out_5849575757077882696[75] = 0;
|
||||
out_5849575757077882696[76] = 0;
|
||||
out_5849575757077882696[77] = 0;
|
||||
out_5849575757077882696[78] = 0;
|
||||
out_5849575757077882696[79] = 0;
|
||||
out_5849575757077882696[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_6955983744751080330) {
|
||||
out_6955983744751080330[0] = 1.0;
|
||||
out_6955983744751080330[1] = 0;
|
||||
out_6955983744751080330[2] = 0;
|
||||
out_6955983744751080330[3] = 0;
|
||||
out_6955983744751080330[4] = 0;
|
||||
out_6955983744751080330[5] = 0;
|
||||
out_6955983744751080330[6] = 0;
|
||||
out_6955983744751080330[7] = 0;
|
||||
out_6955983744751080330[8] = 0;
|
||||
out_6955983744751080330[9] = 0;
|
||||
out_6955983744751080330[10] = 1.0;
|
||||
out_6955983744751080330[11] = 0;
|
||||
out_6955983744751080330[12] = 0;
|
||||
out_6955983744751080330[13] = 0;
|
||||
out_6955983744751080330[14] = 0;
|
||||
out_6955983744751080330[15] = 0;
|
||||
out_6955983744751080330[16] = 0;
|
||||
out_6955983744751080330[17] = 0;
|
||||
out_6955983744751080330[18] = 0;
|
||||
out_6955983744751080330[19] = 0;
|
||||
out_6955983744751080330[20] = 1.0;
|
||||
out_6955983744751080330[21] = 0;
|
||||
out_6955983744751080330[22] = 0;
|
||||
out_6955983744751080330[23] = 0;
|
||||
out_6955983744751080330[24] = 0;
|
||||
out_6955983744751080330[25] = 0;
|
||||
out_6955983744751080330[26] = 0;
|
||||
out_6955983744751080330[27] = 0;
|
||||
out_6955983744751080330[28] = 0;
|
||||
out_6955983744751080330[29] = 0;
|
||||
out_6955983744751080330[30] = 1.0;
|
||||
out_6955983744751080330[31] = 0;
|
||||
out_6955983744751080330[32] = 0;
|
||||
out_6955983744751080330[33] = 0;
|
||||
out_6955983744751080330[34] = 0;
|
||||
out_6955983744751080330[35] = 0;
|
||||
out_6955983744751080330[36] = 0;
|
||||
out_6955983744751080330[37] = 0;
|
||||
out_6955983744751080330[38] = 0;
|
||||
out_6955983744751080330[39] = 0;
|
||||
out_6955983744751080330[40] = 1.0;
|
||||
out_6955983744751080330[41] = 0;
|
||||
out_6955983744751080330[42] = 0;
|
||||
out_6955983744751080330[43] = 0;
|
||||
out_6955983744751080330[44] = 0;
|
||||
out_6955983744751080330[45] = 0;
|
||||
out_6955983744751080330[46] = 0;
|
||||
out_6955983744751080330[47] = 0;
|
||||
out_6955983744751080330[48] = 0;
|
||||
out_6955983744751080330[49] = 0;
|
||||
out_6955983744751080330[50] = 1.0;
|
||||
out_6955983744751080330[51] = 0;
|
||||
out_6955983744751080330[52] = 0;
|
||||
out_6955983744751080330[53] = 0;
|
||||
out_6955983744751080330[54] = 0;
|
||||
out_6955983744751080330[55] = 0;
|
||||
out_6955983744751080330[56] = 0;
|
||||
out_6955983744751080330[57] = 0;
|
||||
out_6955983744751080330[58] = 0;
|
||||
out_6955983744751080330[59] = 0;
|
||||
out_6955983744751080330[60] = 1.0;
|
||||
out_6955983744751080330[61] = 0;
|
||||
out_6955983744751080330[62] = 0;
|
||||
out_6955983744751080330[63] = 0;
|
||||
out_6955983744751080330[64] = 0;
|
||||
out_6955983744751080330[65] = 0;
|
||||
out_6955983744751080330[66] = 0;
|
||||
out_6955983744751080330[67] = 0;
|
||||
out_6955983744751080330[68] = 0;
|
||||
out_6955983744751080330[69] = 0;
|
||||
out_6955983744751080330[70] = 1.0;
|
||||
out_6955983744751080330[71] = 0;
|
||||
out_6955983744751080330[72] = 0;
|
||||
out_6955983744751080330[73] = 0;
|
||||
out_6955983744751080330[74] = 0;
|
||||
out_6955983744751080330[75] = 0;
|
||||
out_6955983744751080330[76] = 0;
|
||||
out_6955983744751080330[77] = 0;
|
||||
out_6955983744751080330[78] = 0;
|
||||
out_6955983744751080330[79] = 0;
|
||||
out_6955983744751080330[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_1306455665935687231) {
|
||||
out_1306455665935687231[0] = state[0];
|
||||
out_1306455665935687231[1] = state[1];
|
||||
out_1306455665935687231[2] = state[2];
|
||||
out_1306455665935687231[3] = state[3];
|
||||
out_1306455665935687231[4] = state[4];
|
||||
out_1306455665935687231[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_1306455665935687231[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_1306455665935687231[7] = state[7];
|
||||
out_1306455665935687231[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_1057256165937700131) {
|
||||
out_1057256165937700131[0] = state[0];
|
||||
out_1057256165937700131[1] = state[1];
|
||||
out_1057256165937700131[2] = state[2];
|
||||
out_1057256165937700131[3] = state[3];
|
||||
out_1057256165937700131[4] = state[4];
|
||||
out_1057256165937700131[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_1057256165937700131[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_1057256165937700131[7] = state[7];
|
||||
out_1057256165937700131[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_1015750461961158607) {
|
||||
out_1015750461961158607[0] = 1;
|
||||
out_1015750461961158607[1] = 0;
|
||||
out_1015750461961158607[2] = 0;
|
||||
out_1015750461961158607[3] = 0;
|
||||
out_1015750461961158607[4] = 0;
|
||||
out_1015750461961158607[5] = 0;
|
||||
out_1015750461961158607[6] = 0;
|
||||
out_1015750461961158607[7] = 0;
|
||||
out_1015750461961158607[8] = 0;
|
||||
out_1015750461961158607[9] = 0;
|
||||
out_1015750461961158607[10] = 1;
|
||||
out_1015750461961158607[11] = 0;
|
||||
out_1015750461961158607[12] = 0;
|
||||
out_1015750461961158607[13] = 0;
|
||||
out_1015750461961158607[14] = 0;
|
||||
out_1015750461961158607[15] = 0;
|
||||
out_1015750461961158607[16] = 0;
|
||||
out_1015750461961158607[17] = 0;
|
||||
out_1015750461961158607[18] = 0;
|
||||
out_1015750461961158607[19] = 0;
|
||||
out_1015750461961158607[20] = 1;
|
||||
out_1015750461961158607[21] = 0;
|
||||
out_1015750461961158607[22] = 0;
|
||||
out_1015750461961158607[23] = 0;
|
||||
out_1015750461961158607[24] = 0;
|
||||
out_1015750461961158607[25] = 0;
|
||||
out_1015750461961158607[26] = 0;
|
||||
out_1015750461961158607[27] = 0;
|
||||
out_1015750461961158607[28] = 0;
|
||||
out_1015750461961158607[29] = 0;
|
||||
out_1015750461961158607[30] = 1;
|
||||
out_1015750461961158607[31] = 0;
|
||||
out_1015750461961158607[32] = 0;
|
||||
out_1015750461961158607[33] = 0;
|
||||
out_1015750461961158607[34] = 0;
|
||||
out_1015750461961158607[35] = 0;
|
||||
out_1015750461961158607[36] = 0;
|
||||
out_1015750461961158607[37] = 0;
|
||||
out_1015750461961158607[38] = 0;
|
||||
out_1015750461961158607[39] = 0;
|
||||
out_1015750461961158607[40] = 1;
|
||||
out_1015750461961158607[41] = 0;
|
||||
out_1015750461961158607[42] = 0;
|
||||
out_1015750461961158607[43] = 0;
|
||||
out_1015750461961158607[44] = 0;
|
||||
out_1015750461961158607[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_1015750461961158607[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_1015750461961158607[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1015750461961158607[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1015750461961158607[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_1015750461961158607[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_1015750461961158607[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_1015750461961158607[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1015750461961158607[53] = -9.8000000000000007*dt;
|
||||
out_1015750461961158607[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_1015750461961158607[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_1015750461961158607[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1015750461961158607[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1015750461961158607[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_1015750461961158607[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_1015750461961158607[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_1015750461961158607[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1015750461961158607[62] = 0;
|
||||
out_1015750461961158607[63] = 0;
|
||||
out_1015750461961158607[64] = 0;
|
||||
out_1015750461961158607[65] = 0;
|
||||
out_1015750461961158607[66] = 0;
|
||||
out_1015750461961158607[67] = 0;
|
||||
out_1015750461961158607[68] = 0;
|
||||
out_1015750461961158607[69] = 0;
|
||||
out_1015750461961158607[70] = 1;
|
||||
out_1015750461961158607[71] = 0;
|
||||
out_1015750461961158607[72] = 0;
|
||||
out_1015750461961158607[73] = 0;
|
||||
out_1015750461961158607[74] = 0;
|
||||
out_1015750461961158607[75] = 0;
|
||||
out_1015750461961158607[76] = 0;
|
||||
out_1015750461961158607[77] = 0;
|
||||
out_1015750461961158607[78] = 0;
|
||||
out_1015750461961158607[79] = 0;
|
||||
out_1015750461961158607[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_2042254547319901591) {
|
||||
out_2042254547319901591[0] = 1;
|
||||
out_2042254547319901591[1] = 0;
|
||||
out_2042254547319901591[2] = 0;
|
||||
out_2042254547319901591[3] = 0;
|
||||
out_2042254547319901591[4] = 0;
|
||||
out_2042254547319901591[5] = 0;
|
||||
out_2042254547319901591[6] = 0;
|
||||
out_2042254547319901591[7] = 0;
|
||||
out_2042254547319901591[8] = 0;
|
||||
out_2042254547319901591[9] = 0;
|
||||
out_2042254547319901591[10] = 1;
|
||||
out_2042254547319901591[11] = 0;
|
||||
out_2042254547319901591[12] = 0;
|
||||
out_2042254547319901591[13] = 0;
|
||||
out_2042254547319901591[14] = 0;
|
||||
out_2042254547319901591[15] = 0;
|
||||
out_2042254547319901591[16] = 0;
|
||||
out_2042254547319901591[17] = 0;
|
||||
out_2042254547319901591[18] = 0;
|
||||
out_2042254547319901591[19] = 0;
|
||||
out_2042254547319901591[20] = 1;
|
||||
out_2042254547319901591[21] = 0;
|
||||
out_2042254547319901591[22] = 0;
|
||||
out_2042254547319901591[23] = 0;
|
||||
out_2042254547319901591[24] = 0;
|
||||
out_2042254547319901591[25] = 0;
|
||||
out_2042254547319901591[26] = 0;
|
||||
out_2042254547319901591[27] = 0;
|
||||
out_2042254547319901591[28] = 0;
|
||||
out_2042254547319901591[29] = 0;
|
||||
out_2042254547319901591[30] = 1;
|
||||
out_2042254547319901591[31] = 0;
|
||||
out_2042254547319901591[32] = 0;
|
||||
out_2042254547319901591[33] = 0;
|
||||
out_2042254547319901591[34] = 0;
|
||||
out_2042254547319901591[35] = 0;
|
||||
out_2042254547319901591[36] = 0;
|
||||
out_2042254547319901591[37] = 0;
|
||||
out_2042254547319901591[38] = 0;
|
||||
out_2042254547319901591[39] = 0;
|
||||
out_2042254547319901591[40] = 1;
|
||||
out_2042254547319901591[41] = 0;
|
||||
out_2042254547319901591[42] = 0;
|
||||
out_2042254547319901591[43] = 0;
|
||||
out_2042254547319901591[44] = 0;
|
||||
out_2042254547319901591[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_2042254547319901591[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_2042254547319901591[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2042254547319901591[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2042254547319901591[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_2042254547319901591[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_2042254547319901591[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_2042254547319901591[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2042254547319901591[53] = -9.8000000000000007*dt;
|
||||
out_2042254547319901591[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_2042254547319901591[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_2042254547319901591[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2042254547319901591[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2042254547319901591[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_2042254547319901591[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_2042254547319901591[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_2042254547319901591[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2042254547319901591[62] = 0;
|
||||
out_2042254547319901591[63] = 0;
|
||||
out_2042254547319901591[64] = 0;
|
||||
out_2042254547319901591[65] = 0;
|
||||
out_2042254547319901591[66] = 0;
|
||||
out_2042254547319901591[67] = 0;
|
||||
out_2042254547319901591[68] = 0;
|
||||
out_2042254547319901591[69] = 0;
|
||||
out_2042254547319901591[70] = 1;
|
||||
out_2042254547319901591[71] = 0;
|
||||
out_2042254547319901591[72] = 0;
|
||||
out_2042254547319901591[73] = 0;
|
||||
out_2042254547319901591[74] = 0;
|
||||
out_2042254547319901591[75] = 0;
|
||||
out_2042254547319901591[76] = 0;
|
||||
out_2042254547319901591[77] = 0;
|
||||
out_2042254547319901591[78] = 0;
|
||||
out_2042254547319901591[79] = 0;
|
||||
out_2042254547319901591[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_579481060641732431) {
|
||||
out_579481060641732431[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_9038137973563738704) {
|
||||
out_9038137973563738704[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_8838814511200069080) {
|
||||
out_8838814511200069080[0] = 0;
|
||||
out_8838814511200069080[1] = 0;
|
||||
out_8838814511200069080[2] = 0;
|
||||
out_8838814511200069080[3] = 0;
|
||||
out_8838814511200069080[4] = 0;
|
||||
out_8838814511200069080[5] = 0;
|
||||
out_8838814511200069080[6] = 1;
|
||||
out_8838814511200069080[7] = 0;
|
||||
out_8838814511200069080[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_6219790086191979796) {
|
||||
out_6219790086191979796[0] = 0;
|
||||
out_6219790086191979796[1] = 0;
|
||||
out_6219790086191979796[2] = 0;
|
||||
out_6219790086191979796[3] = 0;
|
||||
out_6219790086191979796[4] = 0;
|
||||
out_6219790086191979796[5] = 0;
|
||||
out_6219790086191979796[6] = 1;
|
||||
out_6219790086191979796[7] = 0;
|
||||
out_6219790086191979796[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_8628826922044058076) {
|
||||
out_8628826922044058076[0] = state[4];
|
||||
out_8628826922044058076[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_8933773235415263694) {
|
||||
out_8933773235415263694[0] = state[4];
|
||||
out_8933773235415263694[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_7435279963503982970) {
|
||||
out_7435279963503982970[0] = 0;
|
||||
out_7435279963503982970[1] = 0;
|
||||
out_7435279963503982970[2] = 0;
|
||||
out_7435279963503982970[3] = 0;
|
||||
out_7435279963503982970[4] = 1;
|
||||
out_7435279963503982970[5] = 0;
|
||||
out_7435279963503982970[6] = 0;
|
||||
out_7435279963503982970[7] = 0;
|
||||
out_7435279963503982970[8] = 0;
|
||||
out_7435279963503982970[9] = 0;
|
||||
out_7435279963503982970[10] = 0;
|
||||
out_7435279963503982970[11] = 0;
|
||||
out_7435279963503982970[12] = 0;
|
||||
out_7435279963503982970[13] = 0;
|
||||
out_7435279963503982970[14] = 1;
|
||||
out_7435279963503982970[15] = 0;
|
||||
out_7435279963503982970[16] = 0;
|
||||
out_7435279963503982970[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_2661864487389656660) {
|
||||
out_2661864487389656660[0] = 0;
|
||||
out_2661864487389656660[1] = 0;
|
||||
out_2661864487389656660[2] = 0;
|
||||
out_2661864487389656660[3] = 0;
|
||||
out_2661864487389656660[4] = 1;
|
||||
out_2661864487389656660[5] = 0;
|
||||
out_2661864487389656660[6] = 0;
|
||||
out_2661864487389656660[7] = 0;
|
||||
out_2661864487389656660[8] = 0;
|
||||
out_2661864487389656660[9] = 0;
|
||||
out_2661864487389656660[10] = 0;
|
||||
out_2661864487389656660[11] = 0;
|
||||
out_2661864487389656660[12] = 0;
|
||||
out_2661864487389656660[13] = 0;
|
||||
out_2661864487389656660[14] = 1;
|
||||
out_2661864487389656660[15] = 0;
|
||||
out_2661864487389656660[16] = 0;
|
||||
out_2661864487389656660[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_8060396416076809453) {
|
||||
out_8060396416076809453[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_1353345261140371077) {
|
||||
out_1353345261140371077[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_6320481552692820453) {
|
||||
out_6320481552692820453[0] = 0;
|
||||
out_6320481552692820453[1] = 0;
|
||||
out_6320481552692820453[2] = 0;
|
||||
out_6320481552692820453[3] = 0;
|
||||
out_6320481552692820453[4] = 1;
|
||||
out_6320481552692820453[5] = 0;
|
||||
out_6320481552692820453[6] = 0;
|
||||
out_6320481552692820453[7] = 0;
|
||||
out_6320481552692820453[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_3701457127684731169) {
|
||||
out_3701457127684731169[0] = 0;
|
||||
out_3701457127684731169[1] = 0;
|
||||
out_3701457127684731169[2] = 0;
|
||||
out_3701457127684731169[3] = 0;
|
||||
out_3701457127684731169[4] = 1;
|
||||
out_3701457127684731169[5] = 0;
|
||||
out_3701457127684731169[6] = 0;
|
||||
out_3701457127684731169[7] = 0;
|
||||
out_3701457127684731169[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_8663712300753286970) {
|
||||
out_8663712300753286970[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_3791544801975276460) {
|
||||
out_3791544801975276460[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_5866426243635426312) {
|
||||
out_5866426243635426312[0] = 0;
|
||||
out_5866426243635426312[1] = 0;
|
||||
out_5866426243635426312[2] = 0;
|
||||
out_5866426243635426312[3] = 0;
|
||||
out_5866426243635426312[4] = 0;
|
||||
out_5866426243635426312[5] = 0;
|
||||
out_5866426243635426312[6] = 0;
|
||||
out_5866426243635426312[7] = 1;
|
||||
out_5866426243635426312[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_8485450668643515596) {
|
||||
out_8485450668643515596[0] = 0;
|
||||
out_8485450668643515596[1] = 0;
|
||||
out_8485450668643515596[2] = 0;
|
||||
out_8485450668643515596[3] = 0;
|
||||
out_8485450668643515596[4] = 0;
|
||||
out_8485450668643515596[5] = 0;
|
||||
out_8485450668643515596[6] = 0;
|
||||
out_8485450668643515596[7] = 1;
|
||||
out_8485450668643515596[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_2233071042628120952) {
|
||||
out_2233071042628120952[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_4406932832484967267) {
|
||||
out_4406932832484967267[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_4096887481508877236) {
|
||||
out_4096887481508877236[0] = 0;
|
||||
out_4096887481508877236[1] = 0;
|
||||
out_4096887481508877236[2] = 0;
|
||||
out_4096887481508877236[3] = 1;
|
||||
out_4096887481508877236[4] = 0;
|
||||
out_4096887481508877236[5] = 0;
|
||||
out_4096887481508877236[6] = 0;
|
||||
out_4096887481508877236[7] = 0;
|
||||
out_4096887481508877236[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_5876220439485156080) {
|
||||
out_5876220439485156080[0] = 0;
|
||||
out_5876220439485156080[1] = 0;
|
||||
out_5876220439485156080[2] = 0;
|
||||
out_5876220439485156080[3] = 1;
|
||||
out_5876220439485156080[4] = 0;
|
||||
out_5876220439485156080[5] = 0;
|
||||
out_5876220439485156080[6] = 0;
|
||||
out_5876220439485156080[7] = 0;
|
||||
out_5876220439485156080[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_8528880318658063037) {
|
||||
out_8528880318658063037[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_6276545984403911908) {
|
||||
out_6276545984403911908[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_5810250208378428269) {
|
||||
out_5810250208378428269[0] = 0;
|
||||
out_5810250208378428269[1] = 1;
|
||||
out_5810250208378428269[2] = 0;
|
||||
out_5810250208378428269[3] = 0;
|
||||
out_5810250208378428269[4] = 0;
|
||||
out_5810250208378428269[5] = 0;
|
||||
out_5810250208378428269[6] = 0;
|
||||
out_5810250208378428269[7] = 0;
|
||||
out_5810250208378428269[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_3191225783370338985) {
|
||||
out_3191225783370338985[0] = 0;
|
||||
out_3191225783370338985[1] = 1;
|
||||
out_3191225783370338985[2] = 0;
|
||||
out_3191225783370338985[3] = 0;
|
||||
out_3191225783370338985[4] = 0;
|
||||
out_3191225783370338985[5] = 0;
|
||||
out_3191225783370338985[6] = 0;
|
||||
out_3191225783370338985[7] = 0;
|
||||
out_3191225783370338985[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_4837476363436818603) {
|
||||
out_4837476363436818603[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_7011338153293664918) {
|
||||
out_7011338153293664918[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_7554094848261592773) {
|
||||
out_7554094848261592773[0] = 1;
|
||||
out_7554094848261592773[1] = 0;
|
||||
out_7554094848261592773[2] = 0;
|
||||
out_7554094848261592773[3] = 0;
|
||||
out_7554094848261592773[4] = 0;
|
||||
out_7554094848261592773[5] = 0;
|
||||
out_7554094848261592773[6] = 0;
|
||||
out_7554094848261592773[7] = 0;
|
||||
out_7554094848261592773[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_8273624800439869559) {
|
||||
out_8273624800439869559[0] = 1;
|
||||
out_8273624800439869559[1] = 0;
|
||||
out_8273624800439869559[2] = 0;
|
||||
out_8273624800439869559[3] = 0;
|
||||
out_8273624800439869559[4] = 0;
|
||||
out_8273624800439869559[5] = 0;
|
||||
out_8273624800439869559[6] = 0;
|
||||
out_8273624800439869559[7] = 0;
|
||||
out_8273624800439869559[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_7350316286992083367) {
|
||||
out_7350316286992083367[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_8647698245415467134) {
|
||||
out_8647698245415467134[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_5240218141402074836) {
|
||||
out_5240218141402074836[0] = 0;
|
||||
out_5240218141402074836[1] = 0;
|
||||
out_5240218141402074836[2] = 0;
|
||||
out_5240218141402074836[3] = 0;
|
||||
out_5240218141402074836[4] = 0;
|
||||
out_5240218141402074836[5] = 0;
|
||||
out_5240218141402074836[6] = 0;
|
||||
out_5240218141402074836[7] = 0;
|
||||
out_5240218141402074836[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_7859242566410164120) {
|
||||
out_7859242566410164120[0] = 0;
|
||||
out_7859242566410164120[1] = 0;
|
||||
out_7859242566410164120[2] = 0;
|
||||
out_7859242566410164120[3] = 0;
|
||||
out_7859242566410164120[4] = 0;
|
||||
out_7859242566410164120[5] = 0;
|
||||
out_7859242566410164120[6] = 0;
|
||||
out_7859242566410164120[7] = 0;
|
||||
out_7859242566410164120[8] = 1;
|
||||
}
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
|
||||
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
|
||||
}
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_1840749225442711100) {
|
||||
err_fun(nom_x, delta_x, out_1840749225442711100);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7743502251243671447) {
|
||||
err_fun(nom_x, delta_x, out_7743502251243671447);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197) {
|
||||
inv_err_fun(nom_x, true_x, out_7887258947850824197);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8632432640030731669) {
|
||||
inv_err_fun(nom_x, true_x, out_8632432640030731669);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_5849575757077882696) {
|
||||
H_mod_fun(state, out_5849575757077882696);
|
||||
void car_H_mod_fun(double *state, double *out_6955983744751080330) {
|
||||
H_mod_fun(state, out_6955983744751080330);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_1306455665935687231) {
|
||||
f_fun(state, dt, out_1306455665935687231);
|
||||
void car_f_fun(double *state, double dt, double *out_1057256165937700131) {
|
||||
f_fun(state, dt, out_1057256165937700131);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_1015750461961158607) {
|
||||
F_fun(state, dt, out_1015750461961158607);
|
||||
void car_F_fun(double *state, double dt, double *out_2042254547319901591) {
|
||||
F_fun(state, dt, out_2042254547319901591);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_579481060641732431) {
|
||||
h_25(state, unused, out_579481060641732431);
|
||||
void car_h_25(double *state, double *unused, double *out_9038137973563738704) {
|
||||
h_25(state, unused, out_9038137973563738704);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_8838814511200069080) {
|
||||
H_25(state, unused, out_8838814511200069080);
|
||||
void car_H_25(double *state, double *unused, double *out_6219790086191979796) {
|
||||
H_25(state, unused, out_6219790086191979796);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_8628826922044058076) {
|
||||
h_24(state, unused, out_8628826922044058076);
|
||||
void car_h_24(double *state, double *unused, double *out_8933773235415263694) {
|
||||
h_24(state, unused, out_8933773235415263694);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_7435279963503982970) {
|
||||
H_24(state, unused, out_7435279963503982970);
|
||||
void car_H_24(double *state, double *unused, double *out_2661864487389656660) {
|
||||
H_24(state, unused, out_2661864487389656660);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_8060396416076809453) {
|
||||
h_30(state, unused, out_8060396416076809453);
|
||||
void car_h_30(double *state, double *unused, double *out_1353345261140371077) {
|
||||
h_30(state, unused, out_1353345261140371077);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_6320481552692820453) {
|
||||
H_30(state, unused, out_6320481552692820453);
|
||||
void car_H_30(double *state, double *unused, double *out_3701457127684731169) {
|
||||
H_30(state, unused, out_3701457127684731169);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_8663712300753286970) {
|
||||
h_26(state, unused, out_8663712300753286970);
|
||||
void car_h_26(double *state, double *unused, double *out_3791544801975276460) {
|
||||
h_26(state, unused, out_3791544801975276460);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_5866426243635426312) {
|
||||
H_26(state, unused, out_5866426243635426312);
|
||||
void car_H_26(double *state, double *unused, double *out_8485450668643515596) {
|
||||
H_26(state, unused, out_8485450668643515596);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_2233071042628120952) {
|
||||
h_27(state, unused, out_2233071042628120952);
|
||||
void car_h_27(double *state, double *unused, double *out_4406932832484967267) {
|
||||
h_27(state, unused, out_4406932832484967267);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_4096887481508877236) {
|
||||
H_27(state, unused, out_4096887481508877236);
|
||||
void car_H_27(double *state, double *unused, double *out_5876220439485156080) {
|
||||
H_27(state, unused, out_5876220439485156080);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_8528880318658063037) {
|
||||
h_29(state, unused, out_8528880318658063037);
|
||||
void car_h_29(double *state, double *unused, double *out_6276545984403911908) {
|
||||
h_29(state, unused, out_6276545984403911908);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_5810250208378428269) {
|
||||
H_29(state, unused, out_5810250208378428269);
|
||||
void car_H_29(double *state, double *unused, double *out_3191225783370338985) {
|
||||
H_29(state, unused, out_3191225783370338985);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_4837476363436818603) {
|
||||
h_28(state, unused, out_4837476363436818603);
|
||||
void car_h_28(double *state, double *unused, double *out_7011338153293664918) {
|
||||
h_28(state, unused, out_7011338153293664918);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_7554094848261592773) {
|
||||
H_28(state, unused, out_7554094848261592773);
|
||||
void car_H_28(double *state, double *unused, double *out_8273624800439869559) {
|
||||
H_28(state, unused, out_8273624800439869559);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_7350316286992083367) {
|
||||
h_31(state, unused, out_7350316286992083367);
|
||||
void car_h_31(double *state, double *unused, double *out_8647698245415467134) {
|
||||
h_31(state, unused, out_8647698245415467134);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_5240218141402074836) {
|
||||
H_31(state, unused, out_5240218141402074836);
|
||||
void car_H_31(double *state, double *unused, double *out_7859242566410164120) {
|
||||
H_31(state, unused, out_7859242566410164120);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_1840749225442711100);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197);
|
||||
void car_H_mod_fun(double *state, double *out_5849575757077882696);
|
||||
void car_f_fun(double *state, double dt, double *out_1306455665935687231);
|
||||
void car_F_fun(double *state, double dt, double *out_1015750461961158607);
|
||||
void car_h_25(double *state, double *unused, double *out_579481060641732431);
|
||||
void car_H_25(double *state, double *unused, double *out_8838814511200069080);
|
||||
void car_h_24(double *state, double *unused, double *out_8628826922044058076);
|
||||
void car_H_24(double *state, double *unused, double *out_7435279963503982970);
|
||||
void car_h_30(double *state, double *unused, double *out_8060396416076809453);
|
||||
void car_H_30(double *state, double *unused, double *out_6320481552692820453);
|
||||
void car_h_26(double *state, double *unused, double *out_8663712300753286970);
|
||||
void car_H_26(double *state, double *unused, double *out_5866426243635426312);
|
||||
void car_h_27(double *state, double *unused, double *out_2233071042628120952);
|
||||
void car_H_27(double *state, double *unused, double *out_4096887481508877236);
|
||||
void car_h_29(double *state, double *unused, double *out_8528880318658063037);
|
||||
void car_H_29(double *state, double *unused, double *out_5810250208378428269);
|
||||
void car_h_28(double *state, double *unused, double *out_4837476363436818603);
|
||||
void car_H_28(double *state, double *unused, double *out_7554094848261592773);
|
||||
void car_h_31(double *state, double *unused, double *out_7350316286992083367);
|
||||
void car_H_31(double *state, double *unused, double *out_5240218141402074836);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7743502251243671447);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8632432640030731669);
|
||||
void car_H_mod_fun(double *state, double *out_6955983744751080330);
|
||||
void car_f_fun(double *state, double dt, double *out_1057256165937700131);
|
||||
void car_F_fun(double *state, double dt, double *out_2042254547319901591);
|
||||
void car_h_25(double *state, double *unused, double *out_9038137973563738704);
|
||||
void car_H_25(double *state, double *unused, double *out_6219790086191979796);
|
||||
void car_h_24(double *state, double *unused, double *out_8933773235415263694);
|
||||
void car_H_24(double *state, double *unused, double *out_2661864487389656660);
|
||||
void car_h_30(double *state, double *unused, double *out_1353345261140371077);
|
||||
void car_H_30(double *state, double *unused, double *out_3701457127684731169);
|
||||
void car_h_26(double *state, double *unused, double *out_3791544801975276460);
|
||||
void car_H_26(double *state, double *unused, double *out_8485450668643515596);
|
||||
void car_h_27(double *state, double *unused, double *out_4406932832484967267);
|
||||
void car_H_27(double *state, double *unused, double *out_5876220439485156080);
|
||||
void car_h_29(double *state, double *unused, double *out_6276545984403911908);
|
||||
void car_H_29(double *state, double *unused, double *out_3191225783370338985);
|
||||
void car_h_28(double *state, double *unused, double *out_7011338153293664918);
|
||||
void car_H_28(double *state, double *unused, double *out_8273624800439869559);
|
||||
void car_h_31(double *state, double *unused, double *out_8647698245415467134);
|
||||
void car_H_31(double *state, double *unused, double *out_7859242566410164120);
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
void car_set_mass(double x);
|
||||
void car_set_rotational_inertia(double x);
|
||||
|
||||
@@ -17,354 +17,354 @@ const static double MAHA_THRESH_21 = 3.8414588206941227;
|
||||
* *
|
||||
* This file is part of 'ekf' *
|
||||
******************************************************************************/
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_2351947283659598660) {
|
||||
out_2351947283659598660[0] = delta_x[0] + nom_x[0];
|
||||
out_2351947283659598660[1] = delta_x[1] + nom_x[1];
|
||||
out_2351947283659598660[2] = delta_x[2] + nom_x[2];
|
||||
out_2351947283659598660[3] = delta_x[3] + nom_x[3];
|
||||
out_2351947283659598660[4] = delta_x[4] + nom_x[4];
|
||||
out_2351947283659598660[5] = delta_x[5] + nom_x[5];
|
||||
out_2351947283659598660[6] = delta_x[6] + nom_x[6];
|
||||
out_2351947283659598660[7] = delta_x[7] + nom_x[7];
|
||||
out_2351947283659598660[8] = delta_x[8] + nom_x[8];
|
||||
out_2351947283659598660[9] = delta_x[9] + nom_x[9];
|
||||
out_2351947283659598660[10] = delta_x[10] + nom_x[10];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_1734633203049768488) {
|
||||
out_1734633203049768488[0] = delta_x[0] + nom_x[0];
|
||||
out_1734633203049768488[1] = delta_x[1] + nom_x[1];
|
||||
out_1734633203049768488[2] = delta_x[2] + nom_x[2];
|
||||
out_1734633203049768488[3] = delta_x[3] + nom_x[3];
|
||||
out_1734633203049768488[4] = delta_x[4] + nom_x[4];
|
||||
out_1734633203049768488[5] = delta_x[5] + nom_x[5];
|
||||
out_1734633203049768488[6] = delta_x[6] + nom_x[6];
|
||||
out_1734633203049768488[7] = delta_x[7] + nom_x[7];
|
||||
out_1734633203049768488[8] = delta_x[8] + nom_x[8];
|
||||
out_1734633203049768488[9] = delta_x[9] + nom_x[9];
|
||||
out_1734633203049768488[10] = delta_x[10] + nom_x[10];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653) {
|
||||
out_8539310053726918653[0] = -nom_x[0] + true_x[0];
|
||||
out_8539310053726918653[1] = -nom_x[1] + true_x[1];
|
||||
out_8539310053726918653[2] = -nom_x[2] + true_x[2];
|
||||
out_8539310053726918653[3] = -nom_x[3] + true_x[3];
|
||||
out_8539310053726918653[4] = -nom_x[4] + true_x[4];
|
||||
out_8539310053726918653[5] = -nom_x[5] + true_x[5];
|
||||
out_8539310053726918653[6] = -nom_x[6] + true_x[6];
|
||||
out_8539310053726918653[7] = -nom_x[7] + true_x[7];
|
||||
out_8539310053726918653[8] = -nom_x[8] + true_x[8];
|
||||
out_8539310053726918653[9] = -nom_x[9] + true_x[9];
|
||||
out_8539310053726918653[10] = -nom_x[10] + true_x[10];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_7226800202829737504) {
|
||||
out_7226800202829737504[0] = -nom_x[0] + true_x[0];
|
||||
out_7226800202829737504[1] = -nom_x[1] + true_x[1];
|
||||
out_7226800202829737504[2] = -nom_x[2] + true_x[2];
|
||||
out_7226800202829737504[3] = -nom_x[3] + true_x[3];
|
||||
out_7226800202829737504[4] = -nom_x[4] + true_x[4];
|
||||
out_7226800202829737504[5] = -nom_x[5] + true_x[5];
|
||||
out_7226800202829737504[6] = -nom_x[6] + true_x[6];
|
||||
out_7226800202829737504[7] = -nom_x[7] + true_x[7];
|
||||
out_7226800202829737504[8] = -nom_x[8] + true_x[8];
|
||||
out_7226800202829737504[9] = -nom_x[9] + true_x[9];
|
||||
out_7226800202829737504[10] = -nom_x[10] + true_x[10];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_2843304307356815987) {
|
||||
out_2843304307356815987[0] = 1.0;
|
||||
out_2843304307356815987[1] = 0;
|
||||
out_2843304307356815987[2] = 0;
|
||||
out_2843304307356815987[3] = 0;
|
||||
out_2843304307356815987[4] = 0;
|
||||
out_2843304307356815987[5] = 0;
|
||||
out_2843304307356815987[6] = 0;
|
||||
out_2843304307356815987[7] = 0;
|
||||
out_2843304307356815987[8] = 0;
|
||||
out_2843304307356815987[9] = 0;
|
||||
out_2843304307356815987[10] = 0;
|
||||
out_2843304307356815987[11] = 0;
|
||||
out_2843304307356815987[12] = 1.0;
|
||||
out_2843304307356815987[13] = 0;
|
||||
out_2843304307356815987[14] = 0;
|
||||
out_2843304307356815987[15] = 0;
|
||||
out_2843304307356815987[16] = 0;
|
||||
out_2843304307356815987[17] = 0;
|
||||
out_2843304307356815987[18] = 0;
|
||||
out_2843304307356815987[19] = 0;
|
||||
out_2843304307356815987[20] = 0;
|
||||
out_2843304307356815987[21] = 0;
|
||||
out_2843304307356815987[22] = 0;
|
||||
out_2843304307356815987[23] = 0;
|
||||
out_2843304307356815987[24] = 1.0;
|
||||
out_2843304307356815987[25] = 0;
|
||||
out_2843304307356815987[26] = 0;
|
||||
out_2843304307356815987[27] = 0;
|
||||
out_2843304307356815987[28] = 0;
|
||||
out_2843304307356815987[29] = 0;
|
||||
out_2843304307356815987[30] = 0;
|
||||
out_2843304307356815987[31] = 0;
|
||||
out_2843304307356815987[32] = 0;
|
||||
out_2843304307356815987[33] = 0;
|
||||
out_2843304307356815987[34] = 0;
|
||||
out_2843304307356815987[35] = 0;
|
||||
out_2843304307356815987[36] = 1.0;
|
||||
out_2843304307356815987[37] = 0;
|
||||
out_2843304307356815987[38] = 0;
|
||||
out_2843304307356815987[39] = 0;
|
||||
out_2843304307356815987[40] = 0;
|
||||
out_2843304307356815987[41] = 0;
|
||||
out_2843304307356815987[42] = 0;
|
||||
out_2843304307356815987[43] = 0;
|
||||
out_2843304307356815987[44] = 0;
|
||||
out_2843304307356815987[45] = 0;
|
||||
out_2843304307356815987[46] = 0;
|
||||
out_2843304307356815987[47] = 0;
|
||||
out_2843304307356815987[48] = 1.0;
|
||||
out_2843304307356815987[49] = 0;
|
||||
out_2843304307356815987[50] = 0;
|
||||
out_2843304307356815987[51] = 0;
|
||||
out_2843304307356815987[52] = 0;
|
||||
out_2843304307356815987[53] = 0;
|
||||
out_2843304307356815987[54] = 0;
|
||||
out_2843304307356815987[55] = 0;
|
||||
out_2843304307356815987[56] = 0;
|
||||
out_2843304307356815987[57] = 0;
|
||||
out_2843304307356815987[58] = 0;
|
||||
out_2843304307356815987[59] = 0;
|
||||
out_2843304307356815987[60] = 1.0;
|
||||
out_2843304307356815987[61] = 0;
|
||||
out_2843304307356815987[62] = 0;
|
||||
out_2843304307356815987[63] = 0;
|
||||
out_2843304307356815987[64] = 0;
|
||||
out_2843304307356815987[65] = 0;
|
||||
out_2843304307356815987[66] = 0;
|
||||
out_2843304307356815987[67] = 0;
|
||||
out_2843304307356815987[68] = 0;
|
||||
out_2843304307356815987[69] = 0;
|
||||
out_2843304307356815987[70] = 0;
|
||||
out_2843304307356815987[71] = 0;
|
||||
out_2843304307356815987[72] = 1.0;
|
||||
out_2843304307356815987[73] = 0;
|
||||
out_2843304307356815987[74] = 0;
|
||||
out_2843304307356815987[75] = 0;
|
||||
out_2843304307356815987[76] = 0;
|
||||
out_2843304307356815987[77] = 0;
|
||||
out_2843304307356815987[78] = 0;
|
||||
out_2843304307356815987[79] = 0;
|
||||
out_2843304307356815987[80] = 0;
|
||||
out_2843304307356815987[81] = 0;
|
||||
out_2843304307356815987[82] = 0;
|
||||
out_2843304307356815987[83] = 0;
|
||||
out_2843304307356815987[84] = 1.0;
|
||||
out_2843304307356815987[85] = 0;
|
||||
out_2843304307356815987[86] = 0;
|
||||
out_2843304307356815987[87] = 0;
|
||||
out_2843304307356815987[88] = 0;
|
||||
out_2843304307356815987[89] = 0;
|
||||
out_2843304307356815987[90] = 0;
|
||||
out_2843304307356815987[91] = 0;
|
||||
out_2843304307356815987[92] = 0;
|
||||
out_2843304307356815987[93] = 0;
|
||||
out_2843304307356815987[94] = 0;
|
||||
out_2843304307356815987[95] = 0;
|
||||
out_2843304307356815987[96] = 1.0;
|
||||
out_2843304307356815987[97] = 0;
|
||||
out_2843304307356815987[98] = 0;
|
||||
out_2843304307356815987[99] = 0;
|
||||
out_2843304307356815987[100] = 0;
|
||||
out_2843304307356815987[101] = 0;
|
||||
out_2843304307356815987[102] = 0;
|
||||
out_2843304307356815987[103] = 0;
|
||||
out_2843304307356815987[104] = 0;
|
||||
out_2843304307356815987[105] = 0;
|
||||
out_2843304307356815987[106] = 0;
|
||||
out_2843304307356815987[107] = 0;
|
||||
out_2843304307356815987[108] = 1.0;
|
||||
out_2843304307356815987[109] = 0;
|
||||
out_2843304307356815987[110] = 0;
|
||||
out_2843304307356815987[111] = 0;
|
||||
out_2843304307356815987[112] = 0;
|
||||
out_2843304307356815987[113] = 0;
|
||||
out_2843304307356815987[114] = 0;
|
||||
out_2843304307356815987[115] = 0;
|
||||
out_2843304307356815987[116] = 0;
|
||||
out_2843304307356815987[117] = 0;
|
||||
out_2843304307356815987[118] = 0;
|
||||
out_2843304307356815987[119] = 0;
|
||||
out_2843304307356815987[120] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_7212715412967761026) {
|
||||
out_7212715412967761026[0] = 1.0;
|
||||
out_7212715412967761026[1] = 0;
|
||||
out_7212715412967761026[2] = 0;
|
||||
out_7212715412967761026[3] = 0;
|
||||
out_7212715412967761026[4] = 0;
|
||||
out_7212715412967761026[5] = 0;
|
||||
out_7212715412967761026[6] = 0;
|
||||
out_7212715412967761026[7] = 0;
|
||||
out_7212715412967761026[8] = 0;
|
||||
out_7212715412967761026[9] = 0;
|
||||
out_7212715412967761026[10] = 0;
|
||||
out_7212715412967761026[11] = 0;
|
||||
out_7212715412967761026[12] = 1.0;
|
||||
out_7212715412967761026[13] = 0;
|
||||
out_7212715412967761026[14] = 0;
|
||||
out_7212715412967761026[15] = 0;
|
||||
out_7212715412967761026[16] = 0;
|
||||
out_7212715412967761026[17] = 0;
|
||||
out_7212715412967761026[18] = 0;
|
||||
out_7212715412967761026[19] = 0;
|
||||
out_7212715412967761026[20] = 0;
|
||||
out_7212715412967761026[21] = 0;
|
||||
out_7212715412967761026[22] = 0;
|
||||
out_7212715412967761026[23] = 0;
|
||||
out_7212715412967761026[24] = 1.0;
|
||||
out_7212715412967761026[25] = 0;
|
||||
out_7212715412967761026[26] = 0;
|
||||
out_7212715412967761026[27] = 0;
|
||||
out_7212715412967761026[28] = 0;
|
||||
out_7212715412967761026[29] = 0;
|
||||
out_7212715412967761026[30] = 0;
|
||||
out_7212715412967761026[31] = 0;
|
||||
out_7212715412967761026[32] = 0;
|
||||
out_7212715412967761026[33] = 0;
|
||||
out_7212715412967761026[34] = 0;
|
||||
out_7212715412967761026[35] = 0;
|
||||
out_7212715412967761026[36] = 1.0;
|
||||
out_7212715412967761026[37] = 0;
|
||||
out_7212715412967761026[38] = 0;
|
||||
out_7212715412967761026[39] = 0;
|
||||
out_7212715412967761026[40] = 0;
|
||||
out_7212715412967761026[41] = 0;
|
||||
out_7212715412967761026[42] = 0;
|
||||
out_7212715412967761026[43] = 0;
|
||||
out_7212715412967761026[44] = 0;
|
||||
out_7212715412967761026[45] = 0;
|
||||
out_7212715412967761026[46] = 0;
|
||||
out_7212715412967761026[47] = 0;
|
||||
out_7212715412967761026[48] = 1.0;
|
||||
out_7212715412967761026[49] = 0;
|
||||
out_7212715412967761026[50] = 0;
|
||||
out_7212715412967761026[51] = 0;
|
||||
out_7212715412967761026[52] = 0;
|
||||
out_7212715412967761026[53] = 0;
|
||||
out_7212715412967761026[54] = 0;
|
||||
out_7212715412967761026[55] = 0;
|
||||
out_7212715412967761026[56] = 0;
|
||||
out_7212715412967761026[57] = 0;
|
||||
out_7212715412967761026[58] = 0;
|
||||
out_7212715412967761026[59] = 0;
|
||||
out_7212715412967761026[60] = 1.0;
|
||||
out_7212715412967761026[61] = 0;
|
||||
out_7212715412967761026[62] = 0;
|
||||
out_7212715412967761026[63] = 0;
|
||||
out_7212715412967761026[64] = 0;
|
||||
out_7212715412967761026[65] = 0;
|
||||
out_7212715412967761026[66] = 0;
|
||||
out_7212715412967761026[67] = 0;
|
||||
out_7212715412967761026[68] = 0;
|
||||
out_7212715412967761026[69] = 0;
|
||||
out_7212715412967761026[70] = 0;
|
||||
out_7212715412967761026[71] = 0;
|
||||
out_7212715412967761026[72] = 1.0;
|
||||
out_7212715412967761026[73] = 0;
|
||||
out_7212715412967761026[74] = 0;
|
||||
out_7212715412967761026[75] = 0;
|
||||
out_7212715412967761026[76] = 0;
|
||||
out_7212715412967761026[77] = 0;
|
||||
out_7212715412967761026[78] = 0;
|
||||
out_7212715412967761026[79] = 0;
|
||||
out_7212715412967761026[80] = 0;
|
||||
out_7212715412967761026[81] = 0;
|
||||
out_7212715412967761026[82] = 0;
|
||||
out_7212715412967761026[83] = 0;
|
||||
out_7212715412967761026[84] = 1.0;
|
||||
out_7212715412967761026[85] = 0;
|
||||
out_7212715412967761026[86] = 0;
|
||||
out_7212715412967761026[87] = 0;
|
||||
out_7212715412967761026[88] = 0;
|
||||
out_7212715412967761026[89] = 0;
|
||||
out_7212715412967761026[90] = 0;
|
||||
out_7212715412967761026[91] = 0;
|
||||
out_7212715412967761026[92] = 0;
|
||||
out_7212715412967761026[93] = 0;
|
||||
out_7212715412967761026[94] = 0;
|
||||
out_7212715412967761026[95] = 0;
|
||||
out_7212715412967761026[96] = 1.0;
|
||||
out_7212715412967761026[97] = 0;
|
||||
out_7212715412967761026[98] = 0;
|
||||
out_7212715412967761026[99] = 0;
|
||||
out_7212715412967761026[100] = 0;
|
||||
out_7212715412967761026[101] = 0;
|
||||
out_7212715412967761026[102] = 0;
|
||||
out_7212715412967761026[103] = 0;
|
||||
out_7212715412967761026[104] = 0;
|
||||
out_7212715412967761026[105] = 0;
|
||||
out_7212715412967761026[106] = 0;
|
||||
out_7212715412967761026[107] = 0;
|
||||
out_7212715412967761026[108] = 1.0;
|
||||
out_7212715412967761026[109] = 0;
|
||||
out_7212715412967761026[110] = 0;
|
||||
out_7212715412967761026[111] = 0;
|
||||
out_7212715412967761026[112] = 0;
|
||||
out_7212715412967761026[113] = 0;
|
||||
out_7212715412967761026[114] = 0;
|
||||
out_7212715412967761026[115] = 0;
|
||||
out_7212715412967761026[116] = 0;
|
||||
out_7212715412967761026[117] = 0;
|
||||
out_7212715412967761026[118] = 0;
|
||||
out_7212715412967761026[119] = 0;
|
||||
out_7212715412967761026[120] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_774769938980785647) {
|
||||
out_774769938980785647[0] = dt*state[3] + state[0];
|
||||
out_774769938980785647[1] = dt*state[4] + state[1];
|
||||
out_774769938980785647[2] = dt*state[5] + state[2];
|
||||
out_774769938980785647[3] = state[3];
|
||||
out_774769938980785647[4] = state[4];
|
||||
out_774769938980785647[5] = state[5];
|
||||
out_774769938980785647[6] = dt*state[7] + state[6];
|
||||
out_774769938980785647[7] = dt*state[8] + state[7];
|
||||
out_774769938980785647[8] = state[8];
|
||||
out_774769938980785647[9] = state[9];
|
||||
out_774769938980785647[10] = state[10];
|
||||
void f_fun(double *state, double dt, double *out_4622219999608348212) {
|
||||
out_4622219999608348212[0] = dt*state[3] + state[0];
|
||||
out_4622219999608348212[1] = dt*state[4] + state[1];
|
||||
out_4622219999608348212[2] = dt*state[5] + state[2];
|
||||
out_4622219999608348212[3] = state[3];
|
||||
out_4622219999608348212[4] = state[4];
|
||||
out_4622219999608348212[5] = state[5];
|
||||
out_4622219999608348212[6] = dt*state[7] + state[6];
|
||||
out_4622219999608348212[7] = dt*state[8] + state[7];
|
||||
out_4622219999608348212[8] = state[8];
|
||||
out_4622219999608348212[9] = state[9];
|
||||
out_4622219999608348212[10] = state[10];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_4467768114936024642) {
|
||||
out_4467768114936024642[0] = 1;
|
||||
out_4467768114936024642[1] = 0;
|
||||
out_4467768114936024642[2] = 0;
|
||||
out_4467768114936024642[3] = dt;
|
||||
out_4467768114936024642[4] = 0;
|
||||
out_4467768114936024642[5] = 0;
|
||||
out_4467768114936024642[6] = 0;
|
||||
out_4467768114936024642[7] = 0;
|
||||
out_4467768114936024642[8] = 0;
|
||||
out_4467768114936024642[9] = 0;
|
||||
out_4467768114936024642[10] = 0;
|
||||
out_4467768114936024642[11] = 0;
|
||||
out_4467768114936024642[12] = 1;
|
||||
out_4467768114936024642[13] = 0;
|
||||
out_4467768114936024642[14] = 0;
|
||||
out_4467768114936024642[15] = dt;
|
||||
out_4467768114936024642[16] = 0;
|
||||
out_4467768114936024642[17] = 0;
|
||||
out_4467768114936024642[18] = 0;
|
||||
out_4467768114936024642[19] = 0;
|
||||
out_4467768114936024642[20] = 0;
|
||||
out_4467768114936024642[21] = 0;
|
||||
out_4467768114936024642[22] = 0;
|
||||
out_4467768114936024642[23] = 0;
|
||||
out_4467768114936024642[24] = 1;
|
||||
out_4467768114936024642[25] = 0;
|
||||
out_4467768114936024642[26] = 0;
|
||||
out_4467768114936024642[27] = dt;
|
||||
out_4467768114936024642[28] = 0;
|
||||
out_4467768114936024642[29] = 0;
|
||||
out_4467768114936024642[30] = 0;
|
||||
out_4467768114936024642[31] = 0;
|
||||
out_4467768114936024642[32] = 0;
|
||||
out_4467768114936024642[33] = 0;
|
||||
out_4467768114936024642[34] = 0;
|
||||
out_4467768114936024642[35] = 0;
|
||||
out_4467768114936024642[36] = 1;
|
||||
out_4467768114936024642[37] = 0;
|
||||
out_4467768114936024642[38] = 0;
|
||||
out_4467768114936024642[39] = 0;
|
||||
out_4467768114936024642[40] = 0;
|
||||
out_4467768114936024642[41] = 0;
|
||||
out_4467768114936024642[42] = 0;
|
||||
out_4467768114936024642[43] = 0;
|
||||
out_4467768114936024642[44] = 0;
|
||||
out_4467768114936024642[45] = 0;
|
||||
out_4467768114936024642[46] = 0;
|
||||
out_4467768114936024642[47] = 0;
|
||||
out_4467768114936024642[48] = 1;
|
||||
out_4467768114936024642[49] = 0;
|
||||
out_4467768114936024642[50] = 0;
|
||||
out_4467768114936024642[51] = 0;
|
||||
out_4467768114936024642[52] = 0;
|
||||
out_4467768114936024642[53] = 0;
|
||||
out_4467768114936024642[54] = 0;
|
||||
out_4467768114936024642[55] = 0;
|
||||
out_4467768114936024642[56] = 0;
|
||||
out_4467768114936024642[57] = 0;
|
||||
out_4467768114936024642[58] = 0;
|
||||
out_4467768114936024642[59] = 0;
|
||||
out_4467768114936024642[60] = 1;
|
||||
out_4467768114936024642[61] = 0;
|
||||
out_4467768114936024642[62] = 0;
|
||||
out_4467768114936024642[63] = 0;
|
||||
out_4467768114936024642[64] = 0;
|
||||
out_4467768114936024642[65] = 0;
|
||||
out_4467768114936024642[66] = 0;
|
||||
out_4467768114936024642[67] = 0;
|
||||
out_4467768114936024642[68] = 0;
|
||||
out_4467768114936024642[69] = 0;
|
||||
out_4467768114936024642[70] = 0;
|
||||
out_4467768114936024642[71] = 0;
|
||||
out_4467768114936024642[72] = 1;
|
||||
out_4467768114936024642[73] = dt;
|
||||
out_4467768114936024642[74] = 0;
|
||||
out_4467768114936024642[75] = 0;
|
||||
out_4467768114936024642[76] = 0;
|
||||
out_4467768114936024642[77] = 0;
|
||||
out_4467768114936024642[78] = 0;
|
||||
out_4467768114936024642[79] = 0;
|
||||
out_4467768114936024642[80] = 0;
|
||||
out_4467768114936024642[81] = 0;
|
||||
out_4467768114936024642[82] = 0;
|
||||
out_4467768114936024642[83] = 0;
|
||||
out_4467768114936024642[84] = 1;
|
||||
out_4467768114936024642[85] = dt;
|
||||
out_4467768114936024642[86] = 0;
|
||||
out_4467768114936024642[87] = 0;
|
||||
out_4467768114936024642[88] = 0;
|
||||
out_4467768114936024642[89] = 0;
|
||||
out_4467768114936024642[90] = 0;
|
||||
out_4467768114936024642[91] = 0;
|
||||
out_4467768114936024642[92] = 0;
|
||||
out_4467768114936024642[93] = 0;
|
||||
out_4467768114936024642[94] = 0;
|
||||
out_4467768114936024642[95] = 0;
|
||||
out_4467768114936024642[96] = 1;
|
||||
out_4467768114936024642[97] = 0;
|
||||
out_4467768114936024642[98] = 0;
|
||||
out_4467768114936024642[99] = 0;
|
||||
out_4467768114936024642[100] = 0;
|
||||
out_4467768114936024642[101] = 0;
|
||||
out_4467768114936024642[102] = 0;
|
||||
out_4467768114936024642[103] = 0;
|
||||
out_4467768114936024642[104] = 0;
|
||||
out_4467768114936024642[105] = 0;
|
||||
out_4467768114936024642[106] = 0;
|
||||
out_4467768114936024642[107] = 0;
|
||||
out_4467768114936024642[108] = 1;
|
||||
out_4467768114936024642[109] = 0;
|
||||
out_4467768114936024642[110] = 0;
|
||||
out_4467768114936024642[111] = 0;
|
||||
out_4467768114936024642[112] = 0;
|
||||
out_4467768114936024642[113] = 0;
|
||||
out_4467768114936024642[114] = 0;
|
||||
out_4467768114936024642[115] = 0;
|
||||
out_4467768114936024642[116] = 0;
|
||||
out_4467768114936024642[117] = 0;
|
||||
out_4467768114936024642[118] = 0;
|
||||
out_4467768114936024642[119] = 0;
|
||||
out_4467768114936024642[120] = 1;
|
||||
void F_fun(double *state, double dt, double *out_1489675256084431222) {
|
||||
out_1489675256084431222[0] = 1;
|
||||
out_1489675256084431222[1] = 0;
|
||||
out_1489675256084431222[2] = 0;
|
||||
out_1489675256084431222[3] = dt;
|
||||
out_1489675256084431222[4] = 0;
|
||||
out_1489675256084431222[5] = 0;
|
||||
out_1489675256084431222[6] = 0;
|
||||
out_1489675256084431222[7] = 0;
|
||||
out_1489675256084431222[8] = 0;
|
||||
out_1489675256084431222[9] = 0;
|
||||
out_1489675256084431222[10] = 0;
|
||||
out_1489675256084431222[11] = 0;
|
||||
out_1489675256084431222[12] = 1;
|
||||
out_1489675256084431222[13] = 0;
|
||||
out_1489675256084431222[14] = 0;
|
||||
out_1489675256084431222[15] = dt;
|
||||
out_1489675256084431222[16] = 0;
|
||||
out_1489675256084431222[17] = 0;
|
||||
out_1489675256084431222[18] = 0;
|
||||
out_1489675256084431222[19] = 0;
|
||||
out_1489675256084431222[20] = 0;
|
||||
out_1489675256084431222[21] = 0;
|
||||
out_1489675256084431222[22] = 0;
|
||||
out_1489675256084431222[23] = 0;
|
||||
out_1489675256084431222[24] = 1;
|
||||
out_1489675256084431222[25] = 0;
|
||||
out_1489675256084431222[26] = 0;
|
||||
out_1489675256084431222[27] = dt;
|
||||
out_1489675256084431222[28] = 0;
|
||||
out_1489675256084431222[29] = 0;
|
||||
out_1489675256084431222[30] = 0;
|
||||
out_1489675256084431222[31] = 0;
|
||||
out_1489675256084431222[32] = 0;
|
||||
out_1489675256084431222[33] = 0;
|
||||
out_1489675256084431222[34] = 0;
|
||||
out_1489675256084431222[35] = 0;
|
||||
out_1489675256084431222[36] = 1;
|
||||
out_1489675256084431222[37] = 0;
|
||||
out_1489675256084431222[38] = 0;
|
||||
out_1489675256084431222[39] = 0;
|
||||
out_1489675256084431222[40] = 0;
|
||||
out_1489675256084431222[41] = 0;
|
||||
out_1489675256084431222[42] = 0;
|
||||
out_1489675256084431222[43] = 0;
|
||||
out_1489675256084431222[44] = 0;
|
||||
out_1489675256084431222[45] = 0;
|
||||
out_1489675256084431222[46] = 0;
|
||||
out_1489675256084431222[47] = 0;
|
||||
out_1489675256084431222[48] = 1;
|
||||
out_1489675256084431222[49] = 0;
|
||||
out_1489675256084431222[50] = 0;
|
||||
out_1489675256084431222[51] = 0;
|
||||
out_1489675256084431222[52] = 0;
|
||||
out_1489675256084431222[53] = 0;
|
||||
out_1489675256084431222[54] = 0;
|
||||
out_1489675256084431222[55] = 0;
|
||||
out_1489675256084431222[56] = 0;
|
||||
out_1489675256084431222[57] = 0;
|
||||
out_1489675256084431222[58] = 0;
|
||||
out_1489675256084431222[59] = 0;
|
||||
out_1489675256084431222[60] = 1;
|
||||
out_1489675256084431222[61] = 0;
|
||||
out_1489675256084431222[62] = 0;
|
||||
out_1489675256084431222[63] = 0;
|
||||
out_1489675256084431222[64] = 0;
|
||||
out_1489675256084431222[65] = 0;
|
||||
out_1489675256084431222[66] = 0;
|
||||
out_1489675256084431222[67] = 0;
|
||||
out_1489675256084431222[68] = 0;
|
||||
out_1489675256084431222[69] = 0;
|
||||
out_1489675256084431222[70] = 0;
|
||||
out_1489675256084431222[71] = 0;
|
||||
out_1489675256084431222[72] = 1;
|
||||
out_1489675256084431222[73] = dt;
|
||||
out_1489675256084431222[74] = 0;
|
||||
out_1489675256084431222[75] = 0;
|
||||
out_1489675256084431222[76] = 0;
|
||||
out_1489675256084431222[77] = 0;
|
||||
out_1489675256084431222[78] = 0;
|
||||
out_1489675256084431222[79] = 0;
|
||||
out_1489675256084431222[80] = 0;
|
||||
out_1489675256084431222[81] = 0;
|
||||
out_1489675256084431222[82] = 0;
|
||||
out_1489675256084431222[83] = 0;
|
||||
out_1489675256084431222[84] = 1;
|
||||
out_1489675256084431222[85] = dt;
|
||||
out_1489675256084431222[86] = 0;
|
||||
out_1489675256084431222[87] = 0;
|
||||
out_1489675256084431222[88] = 0;
|
||||
out_1489675256084431222[89] = 0;
|
||||
out_1489675256084431222[90] = 0;
|
||||
out_1489675256084431222[91] = 0;
|
||||
out_1489675256084431222[92] = 0;
|
||||
out_1489675256084431222[93] = 0;
|
||||
out_1489675256084431222[94] = 0;
|
||||
out_1489675256084431222[95] = 0;
|
||||
out_1489675256084431222[96] = 1;
|
||||
out_1489675256084431222[97] = 0;
|
||||
out_1489675256084431222[98] = 0;
|
||||
out_1489675256084431222[99] = 0;
|
||||
out_1489675256084431222[100] = 0;
|
||||
out_1489675256084431222[101] = 0;
|
||||
out_1489675256084431222[102] = 0;
|
||||
out_1489675256084431222[103] = 0;
|
||||
out_1489675256084431222[104] = 0;
|
||||
out_1489675256084431222[105] = 0;
|
||||
out_1489675256084431222[106] = 0;
|
||||
out_1489675256084431222[107] = 0;
|
||||
out_1489675256084431222[108] = 1;
|
||||
out_1489675256084431222[109] = 0;
|
||||
out_1489675256084431222[110] = 0;
|
||||
out_1489675256084431222[111] = 0;
|
||||
out_1489675256084431222[112] = 0;
|
||||
out_1489675256084431222[113] = 0;
|
||||
out_1489675256084431222[114] = 0;
|
||||
out_1489675256084431222[115] = 0;
|
||||
out_1489675256084431222[116] = 0;
|
||||
out_1489675256084431222[117] = 0;
|
||||
out_1489675256084431222[118] = 0;
|
||||
out_1489675256084431222[119] = 0;
|
||||
out_1489675256084431222[120] = 1;
|
||||
}
|
||||
void h_6(double *state, double *sat_pos, double *out_8206867657584711155) {
|
||||
out_8206867657584711155[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
|
||||
void h_6(double *state, double *sat_pos, double *out_7567908033892933921) {
|
||||
out_7567908033892933921[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
|
||||
}
|
||||
void H_6(double *state, double *sat_pos, double *out_2748205046550560630) {
|
||||
out_2748205046550560630[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_2748205046550560630[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_2748205046550560630[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_2748205046550560630[3] = 0;
|
||||
out_2748205046550560630[4] = 0;
|
||||
out_2748205046550560630[5] = 0;
|
||||
out_2748205046550560630[6] = 1;
|
||||
out_2748205046550560630[7] = 0;
|
||||
out_2748205046550560630[8] = 0;
|
||||
out_2748205046550560630[9] = 0;
|
||||
out_2748205046550560630[10] = 0;
|
||||
void H_6(double *state, double *sat_pos, double *out_7455993273502557413) {
|
||||
out_7455993273502557413[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_7455993273502557413[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_7455993273502557413[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_7455993273502557413[3] = 0;
|
||||
out_7455993273502557413[4] = 0;
|
||||
out_7455993273502557413[5] = 0;
|
||||
out_7455993273502557413[6] = 1;
|
||||
out_7455993273502557413[7] = 0;
|
||||
out_7455993273502557413[8] = 0;
|
||||
out_7455993273502557413[9] = 0;
|
||||
out_7455993273502557413[10] = 0;
|
||||
}
|
||||
void h_20(double *state, double *sat_pos, double *out_3144072620339266513) {
|
||||
out_3144072620339266513[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
|
||||
void h_20(double *state, double *sat_pos, double *out_3841468768416351937) {
|
||||
out_3841468768416351937[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
|
||||
}
|
||||
void H_20(double *state, double *sat_pos, double *out_8303647801952025828) {
|
||||
out_8303647801952025828[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_8303647801952025828[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_8303647801952025828[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_8303647801952025828[3] = 0;
|
||||
out_8303647801952025828[4] = 0;
|
||||
out_8303647801952025828[5] = 0;
|
||||
out_8303647801952025828[6] = 1;
|
||||
out_8303647801952025828[7] = 0;
|
||||
out_8303647801952025828[8] = 0;
|
||||
out_8303647801952025828[9] = 1;
|
||||
out_8303647801952025828[10] = sat_pos[3];
|
||||
void H_20(double *state, double *sat_pos, double *out_5623941665124898301) {
|
||||
out_5623941665124898301[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_5623941665124898301[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_5623941665124898301[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_5623941665124898301[3] = 0;
|
||||
out_5623941665124898301[4] = 0;
|
||||
out_5623941665124898301[5] = 0;
|
||||
out_5623941665124898301[6] = 1;
|
||||
out_5623941665124898301[7] = 0;
|
||||
out_5623941665124898301[8] = 0;
|
||||
out_5623941665124898301[9] = 1;
|
||||
out_5623941665124898301[10] = sat_pos[3];
|
||||
}
|
||||
void h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
|
||||
out_7730590468597126339[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
|
||||
void h_7(double *state, double *sat_pos_vel, double *out_1211039377275791210) {
|
||||
out_1211039377275791210[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
|
||||
}
|
||||
void H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
|
||||
out_1168235388167943403[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[6] = 0;
|
||||
out_1168235388167943403[7] = 1;
|
||||
out_1168235388167943403[8] = 0;
|
||||
out_1168235388167943403[9] = 0;
|
||||
out_1168235388167943403[10] = 0;
|
||||
void H_7(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
|
||||
out_2159917500592958723[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[6] = 0;
|
||||
out_2159917500592958723[7] = 1;
|
||||
out_2159917500592958723[8] = 0;
|
||||
out_2159917500592958723[9] = 0;
|
||||
out_2159917500592958723[10] = 0;
|
||||
}
|
||||
void h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
|
||||
out_7730590468597126339[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
|
||||
void h_21(double *state, double *sat_pos_vel, double *out_1211039377275791210) {
|
||||
out_1211039377275791210[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
|
||||
}
|
||||
void H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
|
||||
out_1168235388167943403[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_1168235388167943403[6] = 0;
|
||||
out_1168235388167943403[7] = 1;
|
||||
out_1168235388167943403[8] = 0;
|
||||
out_1168235388167943403[9] = 0;
|
||||
out_1168235388167943403[10] = 0;
|
||||
void H_21(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
|
||||
out_2159917500592958723[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[6] = 0;
|
||||
out_2159917500592958723[7] = 1;
|
||||
out_2159917500592958723[8] = 0;
|
||||
out_2159917500592958723[9] = 0;
|
||||
out_2159917500592958723[10] = 0;
|
||||
}
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
@@ -506,44 +506,44 @@ void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
|
||||
update<1, 3, 0>(in_x, in_P, h_21, H_21, NULL, in_z, in_R, in_ea, MAHA_THRESH_21);
|
||||
}
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_2351947283659598660) {
|
||||
err_fun(nom_x, delta_x, out_2351947283659598660);
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_1734633203049768488) {
|
||||
err_fun(nom_x, delta_x, out_1734633203049768488);
|
||||
}
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653) {
|
||||
inv_err_fun(nom_x, true_x, out_8539310053726918653);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_7226800202829737504) {
|
||||
inv_err_fun(nom_x, true_x, out_7226800202829737504);
|
||||
}
|
||||
void gnss_H_mod_fun(double *state, double *out_2843304307356815987) {
|
||||
H_mod_fun(state, out_2843304307356815987);
|
||||
void gnss_H_mod_fun(double *state, double *out_7212715412967761026) {
|
||||
H_mod_fun(state, out_7212715412967761026);
|
||||
}
|
||||
void gnss_f_fun(double *state, double dt, double *out_774769938980785647) {
|
||||
f_fun(state, dt, out_774769938980785647);
|
||||
void gnss_f_fun(double *state, double dt, double *out_4622219999608348212) {
|
||||
f_fun(state, dt, out_4622219999608348212);
|
||||
}
|
||||
void gnss_F_fun(double *state, double dt, double *out_4467768114936024642) {
|
||||
F_fun(state, dt, out_4467768114936024642);
|
||||
void gnss_F_fun(double *state, double dt, double *out_1489675256084431222) {
|
||||
F_fun(state, dt, out_1489675256084431222);
|
||||
}
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_8206867657584711155) {
|
||||
h_6(state, sat_pos, out_8206867657584711155);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_7567908033892933921) {
|
||||
h_6(state, sat_pos, out_7567908033892933921);
|
||||
}
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_2748205046550560630) {
|
||||
H_6(state, sat_pos, out_2748205046550560630);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_7455993273502557413) {
|
||||
H_6(state, sat_pos, out_7455993273502557413);
|
||||
}
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_3144072620339266513) {
|
||||
h_20(state, sat_pos, out_3144072620339266513);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_3841468768416351937) {
|
||||
h_20(state, sat_pos, out_3841468768416351937);
|
||||
}
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_8303647801952025828) {
|
||||
H_20(state, sat_pos, out_8303647801952025828);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_5623941665124898301) {
|
||||
H_20(state, sat_pos, out_5623941665124898301);
|
||||
}
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
|
||||
h_7(state, sat_pos_vel, out_7730590468597126339);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_1211039377275791210) {
|
||||
h_7(state, sat_pos_vel, out_1211039377275791210);
|
||||
}
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
|
||||
H_7(state, sat_pos_vel, out_1168235388167943403);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
|
||||
H_7(state, sat_pos_vel, out_2159917500592958723);
|
||||
}
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
|
||||
h_21(state, sat_pos_vel, out_7730590468597126339);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_1211039377275791210) {
|
||||
h_21(state, sat_pos_vel, out_1211039377275791210);
|
||||
}
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
|
||||
H_21(state, sat_pos_vel, out_1168235388167943403);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
|
||||
H_21(state, sat_pos_vel, out_2159917500592958723);
|
||||
}
|
||||
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user