diff --git a/.gitignore b/.gitignore index 4a9bad64a..5f44371e5 100644 --- a/.gitignore +++ b/.gitignore @@ -50,6 +50,7 @@ selfdrive/sensord/_sensord system/camerad/camerad system/camerad/test/ae_gray_test selfdrive/modeld/_modeld +selfdrive/modeld/_navmodeld selfdrive/modeld/_dmonitoringmodeld /src/ diff --git a/CHANGELOGS.md b/CHANGELOGS.md index d67d40ad7..9571f01ee 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -1,6 +1,6 @@ dragonpilot [Latest] - EON/C2 Release ======================== -* Synced with openpilot master 2022.11.25 commits. +* Synced with openpilot master 2022.12.14 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. @@ -87,6 +87,18 @@ dragonpilot 2022.07.29 - EON/C2 Release dragonpilot [latest] ======================= +* Synced with openpilot master 2022.12.13 commits. +* COMMA: New model, Toyota users can now configure SWAY settings. +* ADDED: Dashcam for comma 3. Store in /data/media/0/video/ (@neokii) +* ADDED: Better TSS2 Long and DF tune. @cgw1968-5779 +* NEW: Toggle to adapt Dynamic End-to-End to Following Dist. Mode. +* NEW: Toggle to adapt Dynamic End-to-End to Accel Mode. +* Improvement: Dynamic end to end is much more reliable and smoother experience +* Bug fixes and improvement. +* Happy Holiday / New Years! Safe Travel, cya in 2023! + +dragonpilot [2022.11.25] +======================= * Synced with openpilot master 2022.11.25 commits. * ADDED: Lateral Controller Selector. - Ability to select between DEFAULT / PID / LQR / Torque Controller. diff --git a/RELEASES.md b/RELEASES.md index 97c92dfa4..7d55887ec 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,7 +1,11 @@ Version 0.9.1 (2022-12-XX) ======================== -* New driving model - +* Adjust alert volume using ambient noise level +* Removed driver monitoring timer resetting on interaction if face detected and distracted +* Chevrolet Bolt EV 2022-23 support thanks to JasonJShuler! +* Genesis GV60 2023 support thanks to sunnyhaibin! +* Hyundai Tucson 2022-23 support +* Kia Sorento Plug-in Hybrid 2022 support thanks to sunnyhaibin! Version 0.9.0 (2022-11-21) ======================== diff --git a/cereal/car.capnp b/cereal/car.capnp index 49665ae8e..47fc84a43 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -34,7 +34,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { buttonCancel @11; buttonEnable @12; pedalPressed @13; # exits active state - pedalPressedPreEnable @73; # added during pre-enable state for either pedal + preEnableStandstill @73; # added during pre-enable state with brake gasPressedOverride @108; # added when user is pressing gas with no disengage on gas steerOverride @114; cruiseDisabled @14; @@ -46,7 +46,6 @@ struct CarEvent @0x9b1657f34caf3ad3 { controlsMismatch @22; pcmEnable @23; pcmDisable @24; - noTarget @25; radarFault @26; brakeHold @28; parkBrake @29; @@ -142,6 +141,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { modelLagWarningDEPRECATED @93; startupOneplusDEPRECATED @82; startupFuzzyFingerprintDEPRECATED @97; + noTargetDEPRECATED @25; } } @@ -327,6 +327,9 @@ struct CarControl { # Actuator commands as computed by controlsd actuators @6 :Actuators; + leftBlinker @15: Bool; + rightBlinker @16: Bool; + # Any car specific rate limits or quirks applied by # the CarController are reflected in actuatorsOutput # and matches what is sent to the car @@ -344,8 +347,12 @@ struct CarControl { brake @1: Float32; # range from -1.0 - 1.0 steer @2: Float32; + # value sent over can to the car + steerOutputCan @8: Float32; steeringAngleDeg @3: Float32; + curvature @7: Float32; + speed @6: Float32; # m/s accel @4: Float32; # m/s^2 longControlState @5: LongControlState; @@ -356,7 +363,6 @@ struct CarControl { stopping @2; starting @3; } - } struct CruiseControl { @@ -596,6 +602,7 @@ struct CarParams { enum SteerControlType { torque @0; angle @1; + curvature @2; } enum TransmissionType { diff --git a/cereal/libcereal_shared.so b/cereal/libcereal_shared.so index 0452bc550..ea7642913 100755 Binary files a/cereal/libcereal_shared.so and b/cereal/libcereal_shared.so differ diff --git a/cereal/log.capnp b/cereal/log.capnp index 492a57c0c..bd39fb9d4 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -655,10 +655,13 @@ struct ControlsState @0x97ff69c53601abf1 { lateralControlState :union { indiState @52 :LateralINDIState; pidState @53 :LateralPIDState; - lqrState @55 :LateralLQRState; angleState @58 :LateralAngleState; debugState @59 :LateralDebugState; torqueState @60 :LateralTorqueState; + curvatureState @65 :LateralCurvatureState; + + #dp + lqrState @55 :LateralLQRState; } enum OpenpilotState @0xdbe58b96d2d1ac61 { @@ -743,6 +746,18 @@ struct ControlsState @0x97ff69c53601abf1 { steeringAngleDesiredDeg @4 :Float32; } + struct LateralCurvatureState { + active @0 :Bool; + actualCurvature @1 :Float32; + desiredCurvature @2 :Float32; + error @3 :Float32; + p @4 :Float32; + i @5 :Float32; + f @6 :Float32; + output @7 :Float32; + saturated @8 :Bool; + } + struct LateralDebugState { active @0 :Bool; steeringAngleDeg @1 :Float32; @@ -2016,6 +2031,30 @@ struct NavRoute { } } +struct MapRenderState { + locationMonoTime @0 :UInt64; + renderTime @1 :Float32; + frameId @2: UInt32; +} + +struct NavModelData { + frameId @0 :UInt32; + modelExecutionTime @1 :Float32; + dspExecutionTime @2 :Float32; + features @3 :List(Float32); + # predicted future position + position @4 :XYData; + desirePrediction @5 :List(Float32); + + # All SI units and in device frame + struct XYData { + x @0 :List(Float32); + y @1 :List(Float32); + xStd @2 :List(Float32); + yStd @3 :List(Float32); + } +} + struct EncodeData { idx @0 :EncodeIndex; data @1 :Data; @@ -2026,6 +2065,15 @@ struct EncodeData { struct UserFlag { } +struct Microphone { + soundPressure @0 :Float32; + + # uncalibrated, A-weighted + soundPressureWeighted @3 :Float32; + soundPressureWeightedDb @1 :Float32; + filteredSoundPressureWeightedDb @2 :Float32; +} + struct Event { logMonoTime @0 :UInt64; # nanoseconds valid @67 :Bool = true; @@ -2075,6 +2123,7 @@ struct Event { liveLocationKalman @72 :LiveLocationKalman; modelV2 @75 :ModelDataV2; driverStateV2 @92 :DriverStateV2; + navModel @104 :NavModelData; # camera stuff, each camera state has a matching encode idx roadCameraState @2 :FrameData; @@ -2085,6 +2134,9 @@ struct Event { wideRoadEncodeIdx @77 :EncodeIndex; qRoadEncodeIdx @90 :EncodeIndex; + # microphone data + microphone @103 :Microphone; + # systems stuff androidLog @20 :AndroidLogEntry; managerState @78 :ManagerState; @@ -2099,11 +2151,12 @@ struct Event { navInstruction @82 :NavInstruction; navRoute @83 :NavRoute; navThumbnail @84: Thumbnail; + mapRenderState @105: MapRenderState; # UI services userFlag @93 :UserFlag; uiDebug @102 :UIDebug; - # dp reserve 103,104 + # dp reserve 105,106 # *********** debug *********** testJoystick @52 :Joystick; roadEncodeData @86 :EncodeData; @@ -2111,8 +2164,8 @@ struct Event { wideRoadEncodeData @88 :EncodeData; qRoadEncodeData @89 :EncodeData; - dragonConf @103 :Dp.DragonConf; - liveMapData @104: LiveMapData; + dragonConf @106 :Dp.DragonConf; + liveMapData @107: LiveMapData; # *********** legacy + deprecated *********** model @9 :Legacy.ModelData; # TODO: rename modelV2 and mark this as deprecated diff --git a/cereal/messaging/bridge b/cereal/messaging/bridge index 180883758..09ddb155f 100755 Binary files a/cereal/messaging/bridge and b/cereal/messaging/bridge differ diff --git a/cereal/messaging/messaging_pyx.so b/cereal/messaging/messaging_pyx.so index e0722375a..a4fb858cc 100755 Binary files a/cereal/messaging/messaging_pyx.so and b/cereal/messaging/messaging_pyx.so differ diff --git a/cereal/services.h b/cereal/services.h index 303b037a8..082210c7b 100644 --- a/cereal/services.h +++ b/cereal/services.h @@ -57,17 +57,20 @@ static struct service services[] = { { "navInstruction", 8053, true, 1, 10 }, { "navRoute", 8054, true, 0, -1 }, { "navThumbnail", 8055, true, 0, -1 }, - { "qRoadEncodeIdx", 8056, false, 20, -1 }, - { "userFlag", 8057, true, 0, 1 }, - { "uiDebug", 8058, true, 0, 1 }, - { "testJoystick", 8059, true, 0, -1 }, - { "roadEncodeData", 8060, false, 20, -1 }, - { "driverEncodeData", 8061, false, 20, -1 }, - { "wideRoadEncodeData", 8062, false, 20, -1 }, - { "qRoadEncodeData", 8063, false, 20, -1 }, - { "dragonConf", 8064, false, 1, -1 }, - { "liveMapData", 8065, true, 0, -1 }, - { "sensorEvents", 8066, true, 100, 100 }, + { "navModel", 8056, true, 2, 4 }, + { "mapRenderState", 8057, true, 2, 1 }, + { "qRoadEncodeIdx", 8058, false, 20, -1 }, + { "userFlag", 8059, true, 0, 1 }, + { "microphone", 8060, true, 10, 10 }, + { "uiDebug", 8061, true, 0, 1 }, + { "testJoystick", 8062, true, 0, -1 }, + { "roadEncodeData", 8063, false, 20, -1 }, + { "driverEncodeData", 8064, false, 20, -1 }, + { "wideRoadEncodeData", 8065, false, 20, -1 }, + { "qRoadEncodeData", 8066, false, 20, -1 }, + { "dragonConf", 8067, false, 1, -1 }, + { "liveMapData", 8068, true, 0, -1 }, + { "sensorEvents", 8069, true, 100, 100 }, }; #endif diff --git a/cereal/services.py b/cereal/services.py index 6d48b694a..bf8f3f0e5 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -75,8 +75,11 @@ services = { "navInstruction": (True, 1., 10), "navRoute": (True, 0.), "navThumbnail": (True, 0.), + "navModel": (True, 2., 4.), + "mapRenderState": (True, 2., 1.), "qRoadEncodeIdx": (False, 20.), "userFlag": (True, 0., 1), + "microphone": (True, 10., 10), # debug "uiDebug": (True, 0., 1), diff --git a/cereal/visionipc/visionipc_pyx.so b/cereal/visionipc/visionipc_pyx.so index dc855397b..366a4f25d 100755 Binary files a/cereal/visionipc/visionipc_pyx.so and b/cereal/visionipc/visionipc_pyx.so differ diff --git a/common/i18n.py b/common/i18n.py index a79a3fc35..7a9130bed 100644 --- a/common/i18n.py +++ b/common/i18n.py @@ -9,6 +9,8 @@ supported_languages = { "main_zh-CHS": "zh-CN", "main_ko": "ko-KR", "main_ja": "ja-JP", + "main_de": "de-DE", + "main_pt-BR": "pt_BR", } def events(): diff --git a/common/params_pyx.so b/common/params_pyx.so index 6e3429229..e346cff4d 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/common/prefix.h b/common/prefix.h new file mode 100644 index 000000000..f5abe14b2 --- /dev/null +++ b/common/prefix.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +#include "common/params.h" +#include "common/util.h" + +class OpenpilotPrefix { +public: + OpenpilotPrefix(std::string prefix = {}) { + if (prefix.empty()) { + prefix = util::random_string(15); + } + msgq_path = "/dev/shm/" + prefix; + bool ret = util::create_directories(msgq_path, 0777); + assert(ret); + setenv("OPENPILOT_PREFIX", prefix.c_str(), 1); + } + + ~OpenpilotPrefix() { + auto param_path = Params().getParamPath(); + if (util::file_exists(param_path)) { + std::string real_path = util::readlink(param_path); + system(util::string_format("rm %s -rf", real_path.c_str()).c_str()); + unlink(param_path.c_str()); + } + system(util::string_format("rm %s -rf", msgq_path.c_str()).c_str()); + unsetenv("OPENPILOT_PREFIX"); + } + +private: + std::string msgq_path; +}; diff --git a/common/util.h b/common/util.h index b46f7bde4..028074384 100644 --- a/common/util.h +++ b/common/util.h @@ -75,6 +75,7 @@ int getenv(const char* key, int default_val); float getenv(const char* key, float default_val); std::string hexdump(const uint8_t* in, const size_t size); +std::string random_string(std::string::size_type length); std::string dir_name(std::string const& path); // **** file fhelpers ***** diff --git a/common/version.h b/common/version.h index 749f75a68..a2c1974ec 100644 --- a/common/version.h +++ b/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "2022.12.23" +#define COMMA_VERSION "2022.12.30" diff --git a/docs/CARS.md b/docs/CARS.md index a0d22a3d2..5c7fe57a8 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,21 +4,22 @@ 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. -# 215 Supported Cars +# 220 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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec| |Acura|RDX 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| -|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| |Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| |Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| +|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| |Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| |Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| |Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| @@ -31,7 +32,8 @@ A supported vehicle is one that just works when you install a comma three. All s |Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F| |Genesis|G80 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| |Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| -|Genesis|GV70 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Genesis|GV60 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| +|Genesis|GV70 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| |GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II| |GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM| |Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A| @@ -59,9 +61,9 @@ A supported vehicle is one that just works when you install a comma three. All s |Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| |Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| |Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J| -|Hyundai|i30 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| -|Hyundai|Ioniq 5 (with HDA II) 2022-23|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q| -|Hyundai|Ioniq 5 (without HDA II) 2022-23|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| +|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| +|Hyundai|Ioniq 5 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q| +|Hyundai|Ioniq 5 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| |Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| |Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| @@ -73,7 +75,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai O| |Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai I| |Hyundai|Palisade 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| -|Hyundai|Santa Cruz 2021-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| +|Hyundai|Santa Cruz 2021-22[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| |Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D| |Hyundai|Santa Fe 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| |Hyundai|Santa Fe Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| @@ -82,14 +84,16 @@ A supported vehicle is one that just works when you install a comma three. All s |Hyundai|Sonata 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| |Hyundai|Sonata Hybrid 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| |Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Hyundai|Tucson 2022[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| +|Hyundai|Tucson 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| |Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| -|Hyundai|Tucson Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| +|Hyundai|Tucson Hybrid 2022[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| |Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| |Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| |Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA| |Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| -|Kia|EV6 (with HDA II) 2022|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P| -|Kia|EV6 (without HDA II) 2022|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| +|Kia|EV6 (with HDA II) 2022[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P| +|Kia|EV6 (without HDA II) 2022[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L| |Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G| |Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| |Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| @@ -104,8 +108,9 @@ A supported vehicle is one that just works when you install a comma three. All s |Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| |Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E| -|Kia|Sportage 2023|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| -|Kia|Sportage Hybrid 2023|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| +|Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A| +|Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| +|Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N| |Kia|Stinger 2018-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C| |Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K| |Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H| @@ -127,17 +132,17 @@ A supported vehicle is one that just works when you install a comma three. All s |Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda| -|Mazda|CX-9 2021-22|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda| +|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda| |Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan B| |Nissan|Leaf 2018-22|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A| |Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A| |Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A| |Ram|1500 2019-22|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ram| -|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| |Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| -|Subaru|Crosstrek 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| @@ -145,13 +150,13 @@ 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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru B| |Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| -|Škoda|Kamiq 2021[6](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Kodiaq 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Škoda|Kamiq 2021[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| +|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Škoda|Kodiaq 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| +|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| |Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| @@ -162,10 +167,10 @@ A supported vehicle is one that just works when you install a comma three. All s |Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|C-HR 2017-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Toyota|C-HR Hybrid 2017-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Toyota|Camry 2018-20|All|Stock|0 mph[5](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Toyota|Camry 2021-22|All|openpilot|0 mph[5](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| +|Toyota|Camry 2021-22|All|openpilot|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| -|Toyota|Camry Hybrid 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| +|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|Corolla 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| @@ -192,48 +197,49 @@ A supported vehicle is one that just works when you install a comma three. All s |Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| |Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota| |Toyota|Sienna 2018-20|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota| -|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat 2015-22[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Polo 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Volkswagen|Polo GTI 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[9](#footnotes)| -|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Tiguan 2019-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| -|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Passat 2015-22[8](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Polo 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| +|Volkswagen|Polo GTI 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| +|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| +|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)| +|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Tiguan 2019-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| +|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533| 1Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`.
2By 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. NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).
3Requires a community built ASCM harness. NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).
42019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
-5openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
-6Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
-7Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
-8Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC.
-9Model-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.
+5Requires a red panda, additional harness box, additional OBD-C cable, USB-A to USB-A cable, and a USB-A to USB-C OTG dongle.
+6openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
+7Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
+8Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
+9Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC.
+10Model-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.
## 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/). diff --git a/docs/INTEGRATION.md b/docs/INTEGRATION.md index 97b72e39d..ba6291c1e 100644 --- a/docs/INTEGRATION.md +++ b/docs/INTEGRATION.md @@ -8,4 +8,4 @@ Additionally, on specific supported cars (see ACC column in [supported cars](CAR * Stock ACC is replaced by openpilot ACC. * openpilot FCW operates in addition to stock FCW. -openpilot should preserve all other vehicle's stock features, including, but are not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning. +openpilot should preserve all other vehicle's stock features, including, but not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning. diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 14b84cc21..ad40e0e5b 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -296,10 +296,15 @@ function launch { # start manager cd selfdrive/manager - if [ ! -f "/data/params/d/OsmLocal" ]; then - ./build.py && ./manager.py + if [ -f /data/params/d/OsmLocal ]; then + OSM_LOCAL=`cat /data/params/d/OsmLocal` else + OSM_LOCAL="0" + fi + if [ $OSM_LOCAL = "1" ]; then ./build.py && ./local_osm_install.py && ./manager.py + else + ./build.py && ./manager.py fi # if broken, keep on screen error diff --git a/opendbc/can/libdbc.so b/opendbc/can/libdbc.so index 52da65f2f..ad0357253 100755 Binary files a/opendbc/can/libdbc.so and b/opendbc/can/libdbc.so differ diff --git a/opendbc/can/parser_pyx.so b/opendbc/can/parser_pyx.so index b65c7e2cc..9fd0fe548 100755 Binary files a/opendbc/can/parser_pyx.so and b/opendbc/can/parser_pyx.so differ diff --git a/opendbc/generator/gm/gm_global_a_powertrain.dbc b/opendbc/generator/gm/gm_global_a_powertrain.dbc index f9b49a697..cad6a73da 100644 --- a/opendbc/generator/gm/gm_global_a_powertrain.dbc +++ b/opendbc/generator/gm/gm_global_a_powertrain.dbc @@ -140,6 +140,7 @@ BO_ 452 AcceleratorPedal2: 8 XXX BO_ 481 ASCMSteeringButton: 7 K124_ASCM SG_ DistanceButton : 22|1@0+ (1,0) [0|0] "" NEO SG_ LKAButton : 23|1@0+ (1,0) [0|0] "" NEO + SG_ ACCAlwaysOne : 24|1@0+ (1,0) [0|1] "" XXX SG_ ACCButtons : 46|3@0+ (1,0) [0|0] "" NEO SG_ DriveModeButton : 39|1@0+ (1,0) [0|1] "" XXX SG_ RollingCounter : 33|2@0+ (1,0) [0|3] "" NEO @@ -285,6 +286,7 @@ CM_ BO_ 190 "Length varies from 6 to 8 bytes by car"; CM_ SG_ 190 GasPedalAndAcc "ACC baseline is 62"; CM_ SG_ 352 Ignition "Non-zero when ignition is on"; CM_ SG_ 451 GasPedalAndAcc2 "ACC baseline is 62"; +CM_ SG_ 481 ACCAlwaysOne "Usually 1 if the car is equipped with ACC"; 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?"; diff --git a/opendbc/generator/toyota/_toyota_2017.dbc b/opendbc/generator/toyota/_toyota_2017.dbc index 6f6f7dd66..1b788b56e 100644 --- a/opendbc/generator/toyota/_toyota_2017.dbc +++ b/opendbc/generator/toyota/_toyota_2017.dbc @@ -83,9 +83,10 @@ BO_ 466 PCM_CRUISE: 8 XXX BO_ 467 PCM_CRUISE_2: 8 XXX SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX - SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX + SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "km/h" XXX + SG_ ACC_FAULTED : 47|1@0+ (1,0) [0|1] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX BO_ 552 ACCELEROMETER: 8 XXX @@ -164,8 +165,9 @@ BO_ 836 PRE_COLLISION_2: 8 DSU SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX BO_ 865 CLUTCH: 8 XXX - SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX + SG_ ACC_FAULTED : 32|1@0+ (1,0) [0|1] "" XXX SG_ GAS_PEDAL_ALT : 23|8@0+ (0.005,0) [0|1] "" XXX + SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX BO_ 869 DSU_CRUISE : 7 DSU SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX @@ -180,6 +182,7 @@ BO_ 921 PCM_CRUISE_SM: 8 XXX SG_ MAIN_ON : 4|1@0+ (1,0) [0|1] "" XXX SG_ CRUISE_CONTROL_STATE : 11|4@0+ (1,0) [0|15] "" XXX SG_ DISTANCE_LINES : 14|2@0+ (1,0) [0|3] "" XXX + SG_ TEMP_ACC_FAULTED : 15|1@0+ (1,0) [0|1] "" XXX SG_ UI_SET_SPEED : 31|8@0+ (1,0) [0|255] "" XXX BO_ 951 ESP_CONTROL: 8 ESP @@ -371,6 +374,7 @@ CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; n CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; +CM_ SG_ 467 ACC_FAULTED "1 when ACC is faulted and the PCM disallows engagement"; CM_ SG_ 467 SET_SPEED "43 km/h are shown as 28 mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -392,7 +396,9 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 865 GAS_PEDAL_ALT "copy of main GAS_PEDAL. Both use 8 bits. Might indicate that this message is for pedals."; CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT."; +CM_ SG_ 865 ACC_FAULTED "1 when ACC is faulted and the PCM disallows engagement"; CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit"; +CM_ SG_ 921 TEMP_ACC_FAULTED "1 when the UI is displaying or playing fault-related alerts or sounds. Also 1 when pressing main on."; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; CM_ SG_ 956 GEAR "on 6MT, only R shows."; CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to"; diff --git a/opendbc/generator/toyota/toyota_nodsu_pt.dbc b/opendbc/generator/toyota/toyota_nodsu_pt.dbc index cbfd1896d..fe0ab3232 100644 --- a/opendbc/generator/toyota/toyota_nodsu_pt.dbc +++ b/opendbc/generator/toyota/toyota_nodsu_pt.dbc @@ -8,10 +8,11 @@ BO_ 401 STEERING_LTA: 8 XXX SG_ SETME_X64 : 47|8@0+ (1,0) [0|255] "" XXX SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX SG_ STEER_ANGLE_CMD : 15|16@0- (0.0573,0) [-540|540] "" XXX - SG_ STEER_REQUEST : 25|1@0+ (1,0) [0|1] "" XXX + SG_ STEER_REQUEST_2 : 25|1@0+ (1,0) [0|1] "" XXX + SG_ LKA_ACTIVE : 26|1@0+ (1,0) [0|1] "" XXX SG_ BIT : 30|1@0+ (1,0) [0|1] "" XXX SG_ COUNTER : 6|6@0+ (1,0) [0|255] "" XXX - SG_ STEER_REQUEST_2 : 0|1@0+ (1,0) [0|1] "" XXX + SG_ STEER_REQUEST : 0|1@0+ (1,0) [0|1] "" XXX SG_ SETME_X1 : 7|1@0+ (1,0) [0|1] "" XXX BO_ 550 BRAKE_MODULE: 8 XXX @@ -38,9 +39,10 @@ CM_ SG_ 401 PERCENTAGE "driver override percentage (0-100), very close to steeri CM_ SG_ 401 SETME_X64 "ramps to 0 smoothly then back on falling edge of STEER_REQUEST if BIT isn't 1"; CM_ SG_ 401 ANGLE "angle of car relative to lane center on LTA camera"; CM_ SG_ 401 STEER_ANGLE_CMD "desired angle, OEM steers up to 95 degrees, no angle limit but torque will bottom out"; -CM_ SG_ 401 STEER_REQUEST "enable bit for steering, 1 to steer, 0 to not"; CM_ SG_ 401 BIT "has correlation to STEER_REQUEST"; +CM_ SG_ 401 STEER_REQUEST "enable bit for steering, 1 to steer, 0 to not"; CM_ SG_ 401 STEER_REQUEST_2 "enable bit for steering, 1 to steer, 0 to not"; +CM_ SG_ 401 LKA_ACTIVE "1 when using LTA for LKA"; CM_ SG_ 550 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 550 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; @@ -52,5 +54,5 @@ CM_ SG_ 1014 ADJACENT_ENABLED "when BSM is enabled in settings, this is on along CM_ SG_ 1014 APPROACHING_ENABLED "when BSM is enabled in settings, this is on along with ADJACENT_ENABLED. this controls bsm alert visibility"; VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled"; -VAL_ 610 LKA_STATE 25 "temporary_fault" 17 "permanent_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; -VAL_ 610 LTA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; +VAL_ 610 LKA_STATE 25 "temporary_fault" 17 "permanent_fault" 11 "lka_missing_unavailable2" 9 "temporary_fault2" 5 "active" 3 "lka_missing_unavailable" 1 "standby"; +VAL_ 610 LTA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 3 "lta_missing_unavailable" 1 "standby"; diff --git a/opendbc/gm_global_a_powertrain_generated.dbc b/opendbc/gm_global_a_powertrain_generated.dbc index 2d823bd4b..a6591d51c 100644 --- a/opendbc/gm_global_a_powertrain_generated.dbc +++ b/opendbc/gm_global_a_powertrain_generated.dbc @@ -160,6 +160,7 @@ BO_ 452 AcceleratorPedal2: 8 XXX BO_ 481 ASCMSteeringButton: 7 K124_ASCM SG_ DistanceButton : 22|1@0+ (1,0) [0|0] "" NEO SG_ LKAButton : 23|1@0+ (1,0) [0|0] "" NEO + SG_ ACCAlwaysOne : 24|1@0+ (1,0) [0|1] "" XXX SG_ ACCButtons : 46|3@0+ (1,0) [0|0] "" NEO SG_ DriveModeButton : 39|1@0+ (1,0) [0|1] "" XXX SG_ RollingCounter : 33|2@0+ (1,0) [0|3] "" NEO @@ -305,6 +306,7 @@ CM_ BO_ 190 "Length varies from 6 to 8 bytes by car"; CM_ SG_ 190 GasPedalAndAcc "ACC baseline is 62"; CM_ SG_ 352 Ignition "Non-zero when ignition is on"; CM_ SG_ 451 GasPedalAndAcc2 "ACC baseline is 62"; +CM_ SG_ 481 ACCAlwaysOne "Usually 1 if the car is equipped with ACC"; 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?"; diff --git a/opendbc/hyundai_canfd.dbc b/opendbc/hyundai_canfd.dbc index 0977f9aef..d58a6c0bf 100644 --- a/opendbc/hyundai_canfd.dbc +++ b/opendbc/hyundai_canfd.dbc @@ -33,7 +33,7 @@ NS_ : BS_: -BU_: XXX CAMERA FRONT_RADAR ADRV +BU_: XXX CAMERA FRONT_RADAR ADRV APRK BO_ 53 ACCELERATOR: 32 XXX @@ -144,6 +144,17 @@ BO_ 304 GEAR_SHIFTER: 16 XXX SG_ GEAR : 64|3@1+ (1,0) [0|7] "" XXX SG_ KNOB_POSITION : 40|3@1+ (1,0) [0|3] "" XXX +BO_ 357 SPAS1: 24 APRK + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|0] "" XXX + SG_ NEW_SIGNAL_1 : 96|16@1- (0.1,0) [0|0] "" XXX + SG_ NEW_SIGNAL_2 : 90|3@1+ (1,0) [0|0] "" XXX + +BO_ 362 SPAS2: 32 APRK + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|0] "" XXX + SG_ BLINKER_CONTROL : 133|3@1+ (1,0) [0|0] "" XXX + BO_ 373 TCS: 24 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX @@ -162,6 +173,7 @@ BO_ 373 TCS: 24 XXX SG_ DriverBrakingLowSens : 84|1@1+ (1,0) [0|1] "" XXX SG_ ACC_REQ : 68|1@0+ (1,0) [0|1] "" XXX SG_ ACCEL_REF_ACC : 48|11@1- (1,0) [0|1023] "" XXX + SG_ ACCEnable : 67|2@0+ (1,0) [0|3] "" XXX BO_ 352 ADRV_0x160: 16 ADRV SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX @@ -238,7 +250,8 @@ BO_ 426 CRUISE_BUTTONS_ALT: 16 XXX SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX SG_ NEW_SIGNAL_1 : 24|4@1+ (1,0) [0|15] "" XXX - SG_ SET_ME_1 : 28|3@1+ (1,0) [0|7] "" XXX + SG_ SET_ME_1 : 28|2@1+ (1,0) [0|3] "" XXX + SG_ DISTANCE_UNIT : 30|1@1+ (1,0) [0|1] "" XXX SG_ NEW_SIGNAL_2 : 31|3@1+ (1,0) [0|7] "" XXX SG_ ADAPTIVE_CRUISE_MAIN_BTN : 34|1@1+ (1,0) [0|1] "" XXX SG_ NEW_SIGNAL_3 : 35|1@1+ (1,0) [0|1] "" XXX @@ -539,6 +552,7 @@ BO_ 506 CLUSTER_SPEED_LIMIT: 32 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_ 373 PROBABLY_EQUIP "aeb equip?"; +CM_ SG_ 373 ACCEnable "Likely a copy of CAN's TCS13->ACCEnable"; CM_ SG_ 373 DriverBraking "Likely derived from BRAKE->BRAKE_POSITION"; CM_ SG_ 373 DriverBrakingLowSens "Higher threshold version of DriverBraking"; CM_ SG_ 352 SET_ME_9 "has something to do with AEB settings"; @@ -556,6 +570,8 @@ 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_ 362 BLINKER_CONTROL 1 "hazards" 2 "hazards button backlight" 3 "left blinkers" 4 "right blinkers"; +VAL_ 373 ACCEnable 0 "SCC ready" 1 "SCC temp fault" 2 "SCC permanent fault" 3 "SCC permanent fault, communication issue"; 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" ; diff --git a/opendbc/toyota_new_mc_pt_generated.dbc b/opendbc/toyota_new_mc_pt_generated.dbc index 708560f64..6b728cd8c 100644 --- a/opendbc/toyota_new_mc_pt_generated.dbc +++ b/opendbc/toyota_new_mc_pt_generated.dbc @@ -128,9 +128,10 @@ BO_ 466 PCM_CRUISE: 8 XXX BO_ 467 PCM_CRUISE_2: 8 XXX SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX - SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX + SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "km/h" XXX + SG_ ACC_FAULTED : 47|1@0+ (1,0) [0|1] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX BO_ 552 ACCELEROMETER: 8 XXX @@ -209,8 +210,9 @@ BO_ 836 PRE_COLLISION_2: 8 DSU SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX BO_ 865 CLUTCH: 8 XXX - SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX + SG_ ACC_FAULTED : 32|1@0+ (1,0) [0|1] "" XXX SG_ GAS_PEDAL_ALT : 23|8@0+ (0.005,0) [0|1] "" XXX + SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX BO_ 869 DSU_CRUISE : 7 DSU SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX @@ -225,6 +227,7 @@ BO_ 921 PCM_CRUISE_SM: 8 XXX SG_ MAIN_ON : 4|1@0+ (1,0) [0|1] "" XXX SG_ CRUISE_CONTROL_STATE : 11|4@0+ (1,0) [0|15] "" XXX SG_ DISTANCE_LINES : 14|2@0+ (1,0) [0|3] "" XXX + SG_ TEMP_ACC_FAULTED : 15|1@0+ (1,0) [0|1] "" XXX SG_ UI_SET_SPEED : 31|8@0+ (1,0) [0|255] "" XXX BO_ 951 ESP_CONTROL: 8 ESP @@ -416,6 +419,7 @@ CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; n CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; +CM_ SG_ 467 ACC_FAULTED "1 when ACC is faulted and the PCM disallows engagement"; CM_ SG_ 467 SET_SPEED "43 km/h are shown as 28 mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -437,7 +441,9 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 865 GAS_PEDAL_ALT "copy of main GAS_PEDAL. Both use 8 bits. Might indicate that this message is for pedals."; CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT."; +CM_ SG_ 865 ACC_FAULTED "1 when ACC is faulted and the PCM disallows engagement"; CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit"; +CM_ SG_ 921 TEMP_ACC_FAULTED "1 when the UI is displaying or playing fault-related alerts or sounds. Also 1 when pressing main on."; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; CM_ SG_ 956 GEAR "on 6MT, only R shows."; CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to"; diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index d9cfd9a7f..6912a31f0 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -128,9 +128,10 @@ BO_ 466 PCM_CRUISE: 8 XXX BO_ 467 PCM_CRUISE_2: 8 XXX SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX - SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX + SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "km/h" XXX + SG_ ACC_FAULTED : 47|1@0+ (1,0) [0|1] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX BO_ 552 ACCELEROMETER: 8 XXX @@ -209,8 +210,9 @@ BO_ 836 PRE_COLLISION_2: 8 DSU SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX BO_ 865 CLUTCH: 8 XXX - SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX + SG_ ACC_FAULTED : 32|1@0+ (1,0) [0|1] "" XXX SG_ GAS_PEDAL_ALT : 23|8@0+ (0.005,0) [0|1] "" XXX + SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX BO_ 869 DSU_CRUISE : 7 DSU SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX @@ -225,6 +227,7 @@ BO_ 921 PCM_CRUISE_SM: 8 XXX SG_ MAIN_ON : 4|1@0+ (1,0) [0|1] "" XXX SG_ CRUISE_CONTROL_STATE : 11|4@0+ (1,0) [0|15] "" XXX SG_ DISTANCE_LINES : 14|2@0+ (1,0) [0|3] "" XXX + SG_ TEMP_ACC_FAULTED : 15|1@0+ (1,0) [0|1] "" XXX SG_ UI_SET_SPEED : 31|8@0+ (1,0) [0|255] "" XXX BO_ 951 ESP_CONTROL: 8 ESP @@ -417,6 +420,7 @@ CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; n CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; +CM_ SG_ 467 ACC_FAULTED "1 when ACC is faulted and the PCM disallows engagement"; CM_ SG_ 467 SET_SPEED "43 km/h are shown as 28 mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -438,7 +442,9 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 865 GAS_PEDAL_ALT "copy of main GAS_PEDAL. Both use 8 bits. Might indicate that this message is for pedals."; CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT."; +CM_ SG_ 865 ACC_FAULTED "1 when ACC is faulted and the PCM disallows engagement"; CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit"; +CM_ SG_ 921 TEMP_ACC_FAULTED "1 when the UI is displaying or playing fault-related alerts or sounds. Also 1 when pressing main on."; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; CM_ SG_ 956 GEAR "on 6MT, only R shows."; CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to"; @@ -541,10 +547,11 @@ BO_ 401 STEERING_LTA: 8 XXX SG_ SETME_X64 : 47|8@0+ (1,0) [0|255] "" XXX SG_ ANGLE : 55|8@0- (0.5,0) [0|255] "" XXX SG_ STEER_ANGLE_CMD : 15|16@0- (0.0573,0) [-540|540] "" XXX - SG_ STEER_REQUEST : 25|1@0+ (1,0) [0|1] "" XXX + SG_ STEER_REQUEST_2 : 25|1@0+ (1,0) [0|1] "" XXX + SG_ LKA_ACTIVE : 26|1@0+ (1,0) [0|1] "" XXX SG_ BIT : 30|1@0+ (1,0) [0|1] "" XXX SG_ COUNTER : 6|6@0+ (1,0) [0|255] "" XXX - SG_ STEER_REQUEST_2 : 0|1@0+ (1,0) [0|1] "" XXX + SG_ STEER_REQUEST : 0|1@0+ (1,0) [0|1] "" XXX SG_ SETME_X1 : 7|1@0+ (1,0) [0|1] "" XXX BO_ 550 BRAKE_MODULE: 8 XXX @@ -571,9 +578,10 @@ CM_ SG_ 401 PERCENTAGE "driver override percentage (0-100), very close to steeri CM_ SG_ 401 SETME_X64 "ramps to 0 smoothly then back on falling edge of STEER_REQUEST if BIT isn't 1"; CM_ SG_ 401 ANGLE "angle of car relative to lane center on LTA camera"; CM_ SG_ 401 STEER_ANGLE_CMD "desired angle, OEM steers up to 95 degrees, no angle limit but torque will bottom out"; -CM_ SG_ 401 STEER_REQUEST "enable bit for steering, 1 to steer, 0 to not"; CM_ SG_ 401 BIT "has correlation to STEER_REQUEST"; +CM_ SG_ 401 STEER_REQUEST "enable bit for steering, 1 to steer, 0 to not"; CM_ SG_ 401 STEER_REQUEST_2 "enable bit for steering, 1 to steer, 0 to not"; +CM_ SG_ 401 LKA_ACTIVE "1 when using LTA for LKA"; CM_ SG_ 550 BRAKE_PRESSURE "seems prop to pedal force"; CM_ SG_ 550 BRAKE_POSITION "seems proportional to pedal displacement, unclear the max value of 0x1c8"; CM_ SG_ 610 TYPE "seems 1 on Corolla, 0 on all others"; @@ -585,5 +593,5 @@ CM_ SG_ 1014 ADJACENT_ENABLED "when BSM is enabled in settings, this is on along CM_ SG_ 1014 APPROACHING_ENABLED "when BSM is enabled in settings, this is on along with ADJACENT_ENABLED. this controls bsm alert visibility"; VAL_ 610 IPAS_STATE 5 "override" 3 "enabled" 1 "disabled"; -VAL_ 610 LKA_STATE 25 "temporary_fault" 17 "permanent_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; -VAL_ 610 LTA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 1 "standby"; +VAL_ 610 LKA_STATE 25 "temporary_fault" 17 "permanent_fault" 11 "lka_missing_unavailable2" 9 "temporary_fault2" 5 "active" 3 "lka_missing_unavailable" 1 "standby"; +VAL_ 610 LTA_STATE 25 "temporary_fault" 9 "temporary_fault2" 5 "active" 3 "lta_missing_unavailable" 1 "standby"; diff --git a/opendbc/toyota_tnga_k_pt_generated.dbc b/opendbc/toyota_tnga_k_pt_generated.dbc index 2eaef636e..a14ae3909 100644 --- a/opendbc/toyota_tnga_k_pt_generated.dbc +++ b/opendbc/toyota_tnga_k_pt_generated.dbc @@ -128,9 +128,10 @@ BO_ 466 PCM_CRUISE: 8 XXX BO_ 467 PCM_CRUISE_2: 8 XXX SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX - SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX + SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX SG_ SET_SPEED : 23|8@0+ (1,0) [0|255] "km/h" XXX + SG_ ACC_FAULTED : 47|1@0+ (1,0) [0|1] "" XXX SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX BO_ 552 ACCELEROMETER: 8 XXX @@ -209,8 +210,9 @@ BO_ 836 PRE_COLLISION_2: 8 DSU SG_ CHECKSUM : 63|8@0+ (1,0) [0|0] "" XXX BO_ 865 CLUTCH: 8 XXX - SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX + SG_ ACC_FAULTED : 32|1@0+ (1,0) [0|1] "" XXX SG_ GAS_PEDAL_ALT : 23|8@0+ (0.005,0) [0|1] "" XXX + SG_ CLUTCH_RELEASED : 38|1@0+ (1,0) [0|1] "" XXX BO_ 869 DSU_CRUISE : 7 DSU SG_ RES_BTN : 3|1@0+ (1,0) [0|0] "" XXX @@ -225,6 +227,7 @@ BO_ 921 PCM_CRUISE_SM: 8 XXX SG_ MAIN_ON : 4|1@0+ (1,0) [0|1] "" XXX SG_ CRUISE_CONTROL_STATE : 11|4@0+ (1,0) [0|15] "" XXX SG_ DISTANCE_LINES : 14|2@0+ (1,0) [0|3] "" XXX + SG_ TEMP_ACC_FAULTED : 15|1@0+ (1,0) [0|1] "" XXX SG_ UI_SET_SPEED : 31|8@0+ (1,0) [0|255] "" XXX BO_ 951 ESP_CONTROL: 8 ESP @@ -416,6 +419,7 @@ CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; n CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; +CM_ SG_ 467 ACC_FAULTED "1 when ACC is faulted and the PCM disallows engagement"; CM_ SG_ 467 SET_SPEED "43 km/h are shown as 28 mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -437,7 +441,9 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; CM_ SG_ 865 GAS_PEDAL_ALT "copy of main GAS_PEDAL. Both use 8 bits. Might indicate that this message is for pedals."; CM_ SG_ 865 CLUTCH_RELEASED "boolean of clutch for 6MT."; +CM_ SG_ 865 ACC_FAULTED "1 when ACC is faulted and the PCM disallows engagement"; CM_ SG_ 921 UI_SET_SPEED "set speed shown in the vehicle's UI with the vehicle's unit"; +CM_ SG_ 921 TEMP_ACC_FAULTED "1 when the UI is displaying or playing fault-related alerts or sounds. Also 1 when pressing main on."; CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel"; CM_ SG_ 956 GEAR "on 6MT, only R shows."; CM_ SG_ 1009 UI_SET_SPEED "units seem to be whatever the car is set to"; diff --git a/panda/.gitignore b/panda/.gitignore index 3a8ead1e0..403d034ee 100644 --- a/panda/.gitignore +++ b/panda/.gitignore @@ -18,3 +18,6 @@ examples/output.csv nosetests.xml .mypy_cache/ .sconsign.dblite + +# CTU info files generated by Cppcheck +*.*.ctu-info diff --git a/panda/__init__.py b/panda/__init__.py index b8d73caf1..12ca88ef1 100644 --- a/panda/__init__.py +++ b/panda/__init__.py @@ -1,7 +1,7 @@ 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) + ALTERNATIVE_EXPERIENCE, USBPACKET_MAX_SIZE) from .python.serial import PandaSerial # noqa: F401 diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index e1404d446..193b41e2f 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 0e6a808c9..60a6a66aa 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda.bin.sspoof.signed b/panda/board/obj/panda.bin.sspoof.signed deleted file mode 100644 index 070ac2e61..000000000 Binary files a/panda/board/obj/panda.bin.sspoof.signed and /dev/null differ diff --git a/panda/board/obj/panda.bin.testing.signed b/panda/board/obj/panda.bin.testing.signed index f1ba65390..5491e1a59 100644 Binary files a/panda/board/obj/panda.bin.testing.signed and b/panda/board/obj/panda.bin.testing.signed differ diff --git a/panda/board/obj/panda.bin.testing.sspoof.signed b/panda/board/obj/panda.bin.testing.sspoof.signed deleted file mode 100644 index 03e5f1e1d..000000000 Binary files a/panda/board/obj/panda.bin.testing.sspoof.signed and /dev/null differ diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 09a6862ba..76c9030c4 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -29,17 +29,18 @@ BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") DEBUG = os.getenv("PANDADEBUG") is not None +CAN_TRANSACTION_MAGIC = struct.pack("= 0x800 else 0 data_len_code = LEN_TO_DLC[len(dat)] header = bytearray(5) @@ -49,57 +50,50 @@ def pack_can_buffer(arr): header[2] = (word_4b >> 8) & 0xFF header[3] = (word_4b >> 16) & 0xFF header[4] = (word_4b >> 24) & 0xFF - snds[idx] += header + dat - if len(snds[idx]) > 256: # Limit chunks to 256 bytes - snds.append(b'') - idx += 1 - # Apply counter to each 64 byte packet - for idx in range(len(snds)): - tx = b'' - counter = 0 - for i in range (0, len(snds[idx]), 63): - tx += bytes([counter]) + snds[idx][i:i+63] - counter += 1 - snds[idx] = tx + snds[-1] += header + dat + if len(snds[-1]) > 256: # Limit chunks to 256 bytes + snds.append(CAN_TRANSACTION_MAGIC) + return snds def unpack_can_buffer(dat): ret = [] - counter = 0 - tail = bytearray() - for i in range(0, len(dat), 64): - if counter != dat[i]: - print("CAN: LOST RECV PACKET COUNTER") - break - counter+=1 - chunk = tail + dat[i+1:i+64] - tail = bytearray() - pos = 0 - while pos>4)] - pckt_len = CANPACKET_HEAD_SIZE + data_len - if pckt_len <= len(chunk[pos:]): - header = chunk[pos:pos+CANPACKET_HEAD_SIZE] - if len(header) < 5: - print("CAN: MALFORMED USB RECV PACKET") - break - bus = (header[0] >> 1) & 0x7 - address = (header[4] << 24 | header[3] << 16 | header[2] << 8 | header[1]) >> 3 - returned = (header[1] >> 1) & 0x1 - rejected = header[1] & 0x1 - data = chunk[pos + CANPACKET_HEAD_SIZE:pos + CANPACKET_HEAD_SIZE + data_len] - if returned: - bus += 128 - if rejected: - bus += 192 - if DEBUG: - print(f" R 0x{address:x}: 0x{data.hex()}") - ret.append((address, 0, data, bus)) - pos += pckt_len - else: - tail = chunk[pos:] - break + if len(dat) < len(CAN_TRANSACTION_MAGIC): + return ret + + if dat[:len(CAN_TRANSACTION_MAGIC)] != CAN_TRANSACTION_MAGIC: + logging.error("CAN: recv didn't start with magic") + return ret + + dat = dat[len(CAN_TRANSACTION_MAGIC):] + + while len(dat) >= CANPACKET_HEAD_SIZE: + data_len = DLC_TO_LEN[(dat[0]>>4)] + + header = dat[:CANPACKET_HEAD_SIZE] + dat = dat[CANPACKET_HEAD_SIZE:] + + bus = (header[0] >> 1) & 0x7 + address = (header[4] << 24 | header[3] << 16 | header[2] << 8 | header[1]) >> 3 + + if (header[1] >> 1) & 0x1: + # returned + bus += 128 + if header[1] & 0x1: + # rejected + bus += 192 + + data = dat[:data_len] + dat = dat[data_len:] + + #logging.debug(" R 0x%x: 0x%s", address, data.hex()) + + ret.append((address, 0, data, bus)) + + if len(dat) > 0: + logging.error("CAN: malformed packet. leftover data") + return ret def ensure_health_packet_version(fn): @@ -167,6 +161,8 @@ class Panda: SAFETY_FAW = 26 SAFETY_BODY = 27 SAFETY_HYUNDAI_CANFD = 28 + SAFETY_VOLVO_C1 = 29 + SAFETY_VOLVO_EUCD = 30 SERIAL_DEBUG = 0 SERIAL_ESP = 1 @@ -191,7 +187,7 @@ class Panda: HW_TYPE_RED_PANDA_V2 = b'\x08' HW_TYPE_TRES = b'\x09' - CAN_PACKET_VERSION = 2 + CAN_PACKET_VERSION = 3 HEALTH_PACKET_VERSION = 11 CAN_HEALTH_PACKET_VERSION = 3 HEALTH_STRUCT = struct.Struct(" UploadFile: + return cls(d.get("fn", ""), d.get("url", ""), d.get("headers", {}), d.get("allow_cellular", False)) + + +@dataclass +class UploadItem: + path: str + url: str + headers: Dict[str, str] + created_at: int + id: Optional[str] + retry_count: int = 0 + current: bool = False + progress: float = 0 + allow_cellular: bool = False + + @classmethod + def from_dict(cls, d: Dict) -> UploadItem: + return cls(d["path"], d["url"], d["headers"], d["created_at"], d["id"], d["retry_count"], d["current"], + d["progress"], d["allow_cellular"]) + + dispatcher["echo"] = lambda s: s -recv_queue: Any = queue.Queue() -send_queue: Any = queue.Queue() -upload_queue: Any = queue.Queue() -low_priority_send_queue: Any = queue.Queue() -log_recv_queue: Any = queue.Queue() -cancelled_uploads: Any = set() -UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count', 'current', 'progress', 'allow_cellular'], defaults=(0, False, 0, False)) +recv_queue: Queue[str] = queue.Queue() +send_queue: Queue[str] = queue.Queue() +upload_queue: Queue[UploadItem] = queue.Queue() +low_priority_send_queue: Queue[str] = queue.Queue() +log_recv_queue: Queue[str] = queue.Queue() +cancelled_uploads: Set[str] = set() -cur_upload_items: Dict[int, Any] = {} +cur_upload_items: Dict[int, Optional[UploadItem]] = {} -def strip_bz2_extension(fn): +def strip_bz2_extension(fn: str) -> str: if fn.endswith('.bz2'): return fn[:-4] return fn @@ -76,29 +114,30 @@ class AbortTransferException(Exception): pass -class UploadQueueCache(): +class UploadQueueCache: params = Params() @staticmethod - def initialize(upload_queue): + def initialize(upload_queue: Queue[UploadItem]) -> None: try: upload_queue_json = UploadQueueCache.params.get("AthenadUploadQueue") if upload_queue_json is not None: for item in json.loads(upload_queue_json): - upload_queue.put(UploadItem(**item)) + upload_queue.put(UploadItem.from_dict(item)) except Exception: cloudlog.exception("athena.UploadQueueCache.initialize.exception") @staticmethod - def cache(upload_queue): + def cache(upload_queue: Queue[UploadItem]) -> None: try: - items = [i._asdict() for i in upload_queue.queue if i.id not in cancelled_uploads] + queue: List[Optional[UploadItem]] = list(upload_queue.queue) + items = [asdict(i) for i in queue if i is not None and (i.id not in cancelled_uploads)] UploadQueueCache.params.put("AthenadUploadQueue", json.dumps(items)) except Exception: cloudlog.exception("athena.UploadQueueCache.cache.exception") -def handle_long_poll(ws): +def handle_long_poll(ws: WebSocket) -> None: end_event = threading.Event() threads = [ @@ -126,7 +165,7 @@ def handle_long_poll(ws): thread.join() -def jsonrpc_handler(end_event): +def jsonrpc_handler(end_event: threading.Event) -> None: dispatcher["startLocalProxy"] = partial(startLocalProxy, end_event) while not end_event.is_set(): try: @@ -147,11 +186,12 @@ def jsonrpc_handler(end_event): def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = True) -> None: - if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT: - item = cur_upload_items[tid] + item = cur_upload_items[tid] + if item is not None and item.retry_count < MAX_RETRY_COUNT: new_retry_count = item.retry_count + 1 if increase_count else item.retry_count - item = item._replace( + item = replace( + item, retry_count=new_retry_count, progress=0, current=False @@ -175,44 +215,44 @@ def upload_handler(end_event: threading.Event) -> None: cur_upload_items[tid] = None try: - cur_upload_items[tid] = upload_queue.get(timeout=1)._replace(current=True) + cur_upload_items[tid] = item = replace(upload_queue.get(timeout=1), current=True) - if cur_upload_items[tid].id in cancelled_uploads: - cancelled_uploads.remove(cur_upload_items[tid].id) + if item.id in cancelled_uploads: + cancelled_uploads.remove(item.id) continue # Remove item if too old - age = datetime.now() - datetime.fromtimestamp(cur_upload_items[tid].created_at / 1000) + age = datetime.now() - datetime.fromtimestamp(item.created_at / 1000) if age.total_seconds() > MAX_AGE: - cloudlog.event("athena.upload_handler.expired", item=cur_upload_items[tid], error=True) + cloudlog.event("athena.upload_handler.expired", item=item, error=True) continue # Check if uploading over metered connection is allowed sm.update(0) metered = sm['deviceState'].networkMetered network_type = sm['deviceState'].networkType.raw - if metered and (not cur_upload_items[tid].allow_cellular): + if metered and (not item.allow_cellular): retry_upload(tid, end_event, False) continue try: - def cb(sz, cur): + def cb(sz: int, cur: int) -> None: # Abort transfer if connection changed to metered after starting upload sm.update(0) metered = sm['deviceState'].networkMetered - if metered and (not cur_upload_items[tid].allow_cellular): + if metered and (not item.allow_cellular): raise AbortTransferException - cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1) + cur_upload_items[tid] = replace(item, progress=cur / sz if sz else 1) - fn = cur_upload_items[tid].path + fn = item.path try: sz = os.path.getsize(fn) except OSError: sz = -1 - cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=cur_upload_items[tid].retry_count) - response = _do_upload(cur_upload_items[tid], cb) + cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered, retry_count=item.retry_count) + response = _do_upload(item, cb) if response.status_code not in (200, 201, 401, 403, 412): cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered) @@ -234,7 +274,7 @@ def upload_handler(end_event: threading.Event) -> None: cloudlog.exception("athena.upload_handler.exception") -def _do_upload(upload_item, callback=None): +def _do_upload(upload_item: UploadItem, callback: Optional[Callable] = None) -> requests.Response: path = upload_item.path compress = False @@ -244,27 +284,25 @@ def _do_upload(upload_item, callback=None): compress = True with open(path, "rb") as f: + data: BinaryIO if compress: cloudlog.event("athena.upload_handler.compress", fn=path, fn_orig=upload_item.path) - data = bz2.compress(f.read()) - size = len(data) - data = io.BytesIO(data) + compressed = bz2.compress(f.read()) + size = len(compressed) + data = io.BytesIO(compressed) else: size = os.fstat(f.fileno()).st_size data = f - if callback: - data = CallbackReader(data, callback, size) - return requests.put(upload_item.url, - data=data, + data=CallbackReader(data, callback, size) if callback else data, headers={**upload_item.headers, 'Content-Length': str(size)}, timeout=30) # security: user should be able to request any message from their car @dispatcher.add_method -def getMessage(service=None, timeout=1000): +def getMessage(service: str, timeout: int = 1000) -> Dict: if service is None or service not in service_list: raise Exception("invalid service") @@ -274,7 +312,8 @@ def getMessage(service=None, timeout=1000): if ret is None: raise TimeoutError - return ret.to_dict() + # this is because capnp._DynamicStructReader doesn't have typing information + return cast(Dict, ret.to_dict()) @dispatcher.add_method @@ -288,7 +327,7 @@ def getVersion() -> Dict[str, str]: @dispatcher.add_method -def setNavDestination(latitude=0, longitude=0, place_name=None, place_details=None): +def setNavDestination(latitude: int = 0, longitude: int = 0, place_name: Optional[str] = None, place_details: Optional[str] = None) -> Dict[str, int]: destination = { "latitude": latitude, "longitude": longitude, @@ -300,8 +339,8 @@ def setNavDestination(latitude=0, longitude=0, place_name=None, place_details=No return {"success": 1} -def scan_dir(path, prefix): - files = list() +def scan_dir(path: str, prefix: str) -> List[str]: + files = [] # only walk directories that match the prefix # (glob and friends traverse entire dir tree) with os.scandir(path) as i: @@ -320,18 +359,18 @@ def scan_dir(path, prefix): return files @dispatcher.add_method -def listDataDirectory(prefix=''): +def listDataDirectory(prefix='') -> List[str]: return scan_dir(ROOT, prefix) @dispatcher.add_method -def reboot(): +def reboot() -> Dict[str, int]: sock = messaging.sub_sock("deviceState", timeout=1000) ret = messaging.recv_one(sock) if ret is None or ret.deviceState.started: raise Exception("Reboot unavailable") - def do_reboot(): + def do_reboot() -> None: time.sleep(2) HARDWARE.reboot() @@ -341,50 +380,53 @@ def reboot(): @dispatcher.add_method -def uploadFileToUrl(fn, url, headers): - return uploadFilesToUrls([{ +def uploadFileToUrl(fn: str, url: str, headers: Dict[str, str]) -> UploadFilesToUrlResponse: + # this is because mypy doesn't understand that the decorator doesn't change the return type + response: UploadFilesToUrlResponse = uploadFilesToUrls([{ "fn": fn, "url": url, "headers": headers, }]) + return response @dispatcher.add_method -def uploadFilesToUrls(files_data): - items = [] - failed = [] - for file in files_data: - fn = file.get('fn', '') - if len(fn) == 0 or fn[0] == '/' or '..' in fn or 'url' not in file: - failed.append(fn) +def uploadFilesToUrls(files_data: List[UploadFileDict]) -> UploadFilesToUrlResponse: + files = map(UploadFile.from_dict, files_data) + + items: List[UploadItemDict] = [] + failed: List[str] = [] + for file in files: + if len(file.fn) == 0 or file.fn[0] == '/' or '..' in file.fn or len(file.url) == 0: + failed.append(file.fn) continue - path = os.path.join(ROOT, fn) + path = os.path.join(ROOT, file.fn) if not os.path.exists(path) and not os.path.exists(strip_bz2_extension(path)): - failed.append(fn) + failed.append(file.fn) continue # Skip item if already in queue - url = file['url'].split('?')[0] + url = file.url.split('?')[0] if any(url == item['url'].split('?')[0] for item in listUploadQueue()): continue item = UploadItem( path=path, - url=file['url'], - headers=file.get('headers', {}), + url=file.url, + headers=file.headers, created_at=int(time.time() * 1000), id=None, - allow_cellular=file.get('allow_cellular', False), + allow_cellular=file.allow_cellular, ) upload_id = hashlib.sha1(str(item).encode()).hexdigest() - item = item._replace(id=upload_id) + item = replace(item, id=upload_id) upload_queue.put_nowait(item) - items.append(item._asdict()) + items.append(asdict(item)) UploadQueueCache.cache(upload_queue) - resp = {"enqueued": len(items), "items": items} + resp: UploadFilesToUrlResponse = {"enqueued": len(items), "items": items} if failed: resp["failed"] = failed @@ -392,32 +434,32 @@ def uploadFilesToUrls(files_data): @dispatcher.add_method -def listUploadQueue(): +def listUploadQueue() -> List[UploadItemDict]: items = list(upload_queue.queue) + list(cur_upload_items.values()) - return [i._asdict() for i in items if (i is not None) and (i.id not in cancelled_uploads)] + return [asdict(i) for i in items if (i is not None) and (i.id not in cancelled_uploads)] @dispatcher.add_method -def cancelUpload(upload_id): +def cancelUpload(upload_id: Union[str, List[str]]) -> Dict[str, Union[int, str]]: if not isinstance(upload_id, list): upload_id = [upload_id] uploading_ids = {item.id for item in list(upload_queue.queue)} cancelled_ids = uploading_ids.intersection(upload_id) if len(cancelled_ids) == 0: - return 404 + return {"success": 0, "error": "not found"} cancelled_uploads.update(cancelled_ids) return {"success": 1} @dispatcher.add_method -def primeActivated(activated): +def primeActivated(activated: bool) -> Dict[str, int]: return {"success": 1} @dispatcher.add_method -def setBandwithLimit(upload_speed_kbps, download_speed_kbps): +def setBandwithLimit(upload_speed_kbps: int, download_speed_kbps: int) -> Dict[str, Union[int, str]]: if not AGNOS: return {"success": 0, "error": "only supported on AGNOS"} @@ -428,7 +470,7 @@ def setBandwithLimit(upload_speed_kbps, download_speed_kbps): return {"success": 0, "error": "failed to set limit", "stdout": e.stdout, "stderr": e.stderr} -def startLocalProxy(global_end_event, remote_ws_uri, local_port): +def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local_port: int) -> Dict[str, int]: try: if local_port not in LOCAL_PORT_WHITELIST: raise Exception("Requested local port not whitelisted") @@ -462,7 +504,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port): @dispatcher.add_method -def getPublicKey(): +def getPublicKey() -> Optional[str]: if not os.path.isfile(PERSIST + '/comma/id_rsa.pub'): return None @@ -471,7 +513,7 @@ def getPublicKey(): @dispatcher.add_method -def getSshAuthorizedKeys(): +def getSshAuthorizedKeys() -> str: return Params().get("GithubSshKeys", encoding='utf8') or '' @@ -486,7 +528,7 @@ def getNetworkType(): @dispatcher.add_method -def getNetworkMetered(): +def getNetworkMetered() -> bool: network_type = HARDWARE.get_network_type() return HARDWARE.get_network_metered(network_type) @@ -497,7 +539,7 @@ def getNetworks(): @dispatcher.add_method -def takeSnapshot(): +def takeSnapshot() -> Optional[Union[str, Dict[str, str]]]: from system.camerad.snapshot.snapshot import jpeg_write, snapshot ret = snapshot() if ret is not None: @@ -514,16 +556,19 @@ def takeSnapshot(): raise Exception("not available while camerad is started") -def get_logs_to_send_sorted(): +def get_logs_to_send_sorted() -> List[str]: # TODO: scan once then use inotify to detect file creation/deletion curr_time = int(time.time()) logs = [] for log_entry in os.listdir(SWAGLOG_DIR): log_path = os.path.join(SWAGLOG_DIR, log_entry) + time_sent = 0 try: - time_sent = int.from_bytes(getxattr(log_path, LOG_ATTR_NAME), sys.byteorder) + value = getxattr(log_path, LOG_ATTR_NAME) + if value is not None: + time_sent = int.from_bytes(value, sys.byteorder) except (ValueError, TypeError): - time_sent = 0 + pass # assume send failed and we lost the response if sent more than one hour ago if not time_sent or curr_time - time_sent > 3600: logs.append(log_entry) @@ -531,7 +576,7 @@ def get_logs_to_send_sorted(): return sorted(logs)[:-1] -def log_handler(end_event): +def log_handler(end_event: threading.Event) -> None: # dont upload any logs for now if True: return @@ -594,7 +639,7 @@ def log_handler(end_event): cloudlog.exception("athena.log_handler.exception") -def stat_handler(end_event): +def stat_handler(end_event: threading.Event) -> None: while not end_event.is_set(): last_scan = 0 curr_scan = sec_since_boot() @@ -620,7 +665,7 @@ def stat_handler(end_event): time.sleep(0.1) -def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event): +def ws_proxy_recv(ws: WebSocket, local_sock: socket.socket, ssock: socket.socket, end_event: threading.Event, global_end_event: threading.Event) -> None: while not (end_event.is_set() or global_end_event.is_set()): try: data = ws.recv() @@ -639,7 +684,7 @@ def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event): end_event.set() -def ws_proxy_send(ws, local_sock, signal_sock, end_event): +def ws_proxy_send(ws: WebSocket, local_sock: socket.socket, signal_sock: socket.socket, end_event: threading.Event) -> None: while not end_event.is_set(): try: r, _, _ = select.select((local_sock, signal_sock), (), ()) @@ -664,7 +709,7 @@ def ws_proxy_send(ws, local_sock, signal_sock, end_event): cloudlog.debug("athena.ws_proxy_send done closing sockets") -def ws_recv(ws, end_event): +def ws_recv(ws: WebSocket, end_event: threading.Event) -> None: last_ping = int(sec_since_boot() * 1e9) while not end_event.is_set(): try: @@ -686,7 +731,7 @@ def ws_recv(ws, end_event): end_event.set() -def ws_send(ws, end_event): +def ws_send(ws: WebSocket, end_event: threading.Event) -> None: while not end_event.is_set(): try: try: @@ -705,7 +750,7 @@ def ws_send(ws, end_event): end_event.set() -def backoff(retries): +def backoff(retries: int) -> int: return random.randrange(0, min(128, int(2 ** retries))) diff --git a/selfdrive/boardd/boardd b/selfdrive/boardd/boardd index 299f76c6c..9675acf3f 100755 Binary files a/selfdrive/boardd/boardd and b/selfdrive/boardd/boardd differ diff --git a/selfdrive/boardd/boardd_api_impl.so b/selfdrive/boardd/boardd_api_impl.so index 20672e558..3dc27c8cd 100755 Binary files a/selfdrive/boardd/boardd_api_impl.so and b/selfdrive/boardd/boardd_api_impl.so differ diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index ffe071ff4..0f3d3b1e2 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -1,30 +1,26 @@ #pragma once -#include #include #include #include #include -#include +#include #include #include -#include - #include "cereal/gen/cpp/car.capnp.h" #include "cereal/gen/cpp/log.capnp.h" #include "panda/board/health.h" +#include "panda/board/can_definitions.h" +#include "selfdrive/boardd/panda_comms.h" -#define TIMEOUT 0 -#define PANDA_CAN_CNT 3 -#define PANDA_BUS_CNT 4 -#define RECV_SIZE (0x4000U) #define USB_TX_SOFT_LIMIT (0x100U) #define USBPACKET_MAX_SIZE (0x40) -#define CANPACKET_HEAD_SIZE 5U -#define CANPACKET_MAX_SIZE 72U -#define CANPACKET_REJECTED (0xC0U) -#define CANPACKET_RETURNED (0x80U) + +#define RECV_SIZE (0x4000U) + +#define CAN_REJECTED_BUS_OFFSET 0xC0U +#define CAN_RETURNED_BUS_OFFSET 0x80U struct __attribute__((packed)) can_header { uint8_t reserved : 1; @@ -37,42 +33,31 @@ struct __attribute__((packed)) can_header { }; struct can_frame { - long address; - std::string dat; - long busTime; - long src; + long address; + std::string dat; + long busTime; + long src; }; + class Panda { - private: - libusb_context *ctx = NULL; - libusb_device_handle *dev_handle = NULL; - std::mutex usb_lock; - std::vector recv_buf; - void handle_usb_issue(int err, const char func[]); - void cleanup(); +private: + std::unique_ptr handle; - public: +public: Panda(std::string serial="", uint32_t bus_offset=0); - ~Panda(); - std::string usb_serial; - std::atomic connected = true; - std::atomic comms_healthy = true; + std::string hw_serial; cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; bool has_rtc = false; const uint32_t bus_offset; - bool has_gps = true; + + bool connected(); + bool comms_healthy(); // Static functions static std::vector list(); - // HW communication - int usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout=TIMEOUT); - int usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout=TIMEOUT); - int usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - int usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - // Panda functionality cereal::PandaState::PandaType get_hw_type(); void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U); @@ -93,8 +78,16 @@ 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::Reader can_data_list); bool can_receive(std::vector& out_vec); + // dp + void set_pigeon_baud(int baud); + bool has_gps = true; + // dp - passthru for pigeon + int control_write(uint8_t request, uint16_t param1, uint16_t param2); + int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length); + int bulk_write(unsigned char endpoint, unsigned char* data, int length); protected: // for unit tests diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h index f42eadc5b..bd262dfa0 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/boardd/panda_comms.h @@ -13,8 +13,6 @@ #define TIMEOUT 0 #define SPI_BUF_SIZE 1024 -const bool PANDA_NO_RETRY = getenv("PANDA_NO_RETRY"); - // comms base class class PandaCommsHandle { @@ -52,7 +50,6 @@ public: private: libusb_context *ctx = NULL; libusb_device_handle *dev_handle = NULL; - std::vector recv_buf; void handle_usb_issue(int err, const char func[]); }; diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 491c551b1..c056f7ca3 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,8 +1,9 @@ # functions common among cars import capnp +from collections import namedtuple from cereal import car -from common.numpy_fast import clip +from common.numpy_fast import clip, interp from typing import Dict # kg of standard extra cargo to count for drive, gas, etc... @@ -10,6 +11,7 @@ STD_CARGO_KG = 136. ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +AngleRateLimit = namedtuple('AngleRateLimit', ['speed_bp', 'angle_v']) def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float: @@ -111,6 +113,15 @@ def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torq return int(round(float(apply_torque))) +def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, LIMITS): + # pick angle rate limits based on wind up/down + steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last) + rate_limits = LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else LIMITS.ANGLE_RATE_LIMIT_DOWN + + angle_rate_lim = interp(v_ego, rate_limits.speed_bp, rate_limits.angle_v) + return clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim) + + def crc8_pedal(data): crc = 0xFF # standard init value poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1 diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 0d5d780bd..00673a7e2 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -85,6 +85,7 @@ class CarController: new_actuators = CC.actuators.copy() new_actuators.accel = torque_l new_actuators.steer = torque_r + new_actuators.steerOutputCan = torque_r self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index ae7ab89aa..bc5b36e2e 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -2,16 +2,13 @@ import math from cereal import car from common.realtime import DT_CTRL -from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config +from selfdrive.car import get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.body.values import SPEED_FROM_RPM class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=None, car_fw=None, experimental_long=False): - - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.notCar = True ret.carName = "body" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] @@ -31,10 +28,6 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = True ret.steerControlType = car.CarParams.SteerControlType.angle - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - return ret def _update(self, c): diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 5a2d90c64..31cd01b65 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -2,7 +2,7 @@ from opendbc.can.packer import CANPacker from common.realtime import DT_CTRL from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, create_cruise_buttons -from selfdrive.car.chrysler.values import CAR, RAM_CARS, CarControllerParams +from selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags class CarController: @@ -51,7 +51,7 @@ class CarController: lkas_control_bit = self.lkas_control_bit_prev if CS.out.vEgo > self.CP.minSteerSpeed: lkas_control_bit = True - elif self.CP.carFingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): + elif self.CP.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED: if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0): lkas_control_bit = False elif self.CP.carFingerprint in RAM_CARS: @@ -78,5 +78,6 @@ class CarController: new_actuators = CC.actuators.copy() new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last return new_actuators, can_sends diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 6344242ff..6d45666d2 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,21 +1,18 @@ #!/usr/bin/env python3 from cereal import car from panda import Panda -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config -from selfdrive.car.chrysler.values import CAR, DBC, RAM_HD, RAM_DT +from selfdrive.car import STD_CARGO_KG, get_safety_config +from selfdrive.car.chrysler.values import CAR, DBC, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "chrysler" - ret.dashcamOnly = candidate in RAM_HD ret.radarOffCan = DBC[candidate]['radar'] is None - ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 @@ -27,9 +24,12 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_DT ret.minSteerSpeed = 3.8 # m/s - if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): - # TODO: allow 2019 cars to steer down to 13 m/s if already engaged. - ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. + if candidate not in RAM_CARS: + # Newer FW versions standard on the following platforms, or flashed by a dealer onto older platforms have a higher minimum steering speed. + new_eps_platform = candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019) + new_eps_firmware = any(fw.ecu == 'eps' and fw.fwVersion[:4] >= b"6841" for fw in car_fw) + if new_eps_platform or new_eps_firmware: + ret.flags |= ChryslerFlags.HIGHER_MIN_STEERING_SPEED.value # Chrysler if candidate in (CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020): @@ -58,10 +58,9 @@ class CarInterface(CarInterfaceBase): ret.mass = 2493. + STD_CARGO_KG CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) ret.minSteerSpeed = 14.5 - if car_fw is not None: - for fw in car_fw: - if fw.ecu == 'eps' and fw.fwVersion[:8] in (b"68312176", b"68273275"): - ret.minSteerSpeed = 0. + for fw in car_fw: + if fw.ecu == 'eps' and fw.fwVersion.startswith((b"68312176", b"68273275")): + ret.minSteerSpeed = 0. elif candidate == CAR.RAM_HD: ret.steerActuatorDelay = 0.2 @@ -75,16 +74,12 @@ class CarInterface(CarInterfaceBase): raise ValueError(f"Unsupported car: {candidate}") CarInterfaceBase.configure_dp_tune(candidate, ret.lateralTuning) + + if ret.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED: + # TODO: allow these cars to steer down to 13 m/s if already engaged. + ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. ret.centerToFront = ret.wheelbase * 0.44 - - # starting with reasonable value for civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by - # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - ret.enableBsm = 720 in fingerprint[0] return ret diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 16530ed98..02261a0b6 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -1,3 +1,4 @@ +from enum import IntFlag from dataclasses import dataclass from enum import Enum from typing import Dict, List, Optional, Union @@ -11,6 +12,10 @@ from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu +class ChryslerFlags(IntFlag): + HIGHER_MIN_STEERING_SPEED = 1 + + class CAR: # Chrysler PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017" diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index 7cf44514d..03e9e721b 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -68,7 +68,7 @@ class Harness(Enum): none = "None" -CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only"], defaults=(None, False)) +CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only", "shop_footnote"], defaults=(False, False)) class CommonFootnote(Enum): diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 8ef161954..0885151cc 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from common.conversions import Conversions as CV -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.ford.values import CAR, Ecu, TransmissionType, GearShifter from selfdrive.car.interfaces import CarInterfaceBase @@ -10,12 +10,7 @@ CarParams = car.CarParams class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - if car_fw is None: - car_fw = [] - - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "ford" ret.dashcamOnly = True ret.safetyConfigs = [get_safety_config(CarParams.SafetyModel.ford)] @@ -24,7 +19,6 @@ class CarInterface(CarInterfaceBase): ret.steerControlType = CarParams.SteerControlType.angle ret.steerActuatorDelay = 0.4 ret.steerLimitTimer = 1.0 - tire_stiffness_factor = 1.0 if candidate == CAR.ESCAPE_MK4: ret.wheelbase = 2.71 @@ -62,10 +56,7 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = 0. ret.autoResumeSng = ret.minEnableSpeed == -1. - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) ret.centerToFront = ret.wheelbase * 0.44 - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, - tire_stiffness_factor=tire_stiffness_factor) return ret def _update(self, c): diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index a6cd2f19b..d380abf8e 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -158,6 +158,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last new_actuators.gas = self.apply_gas new_actuators.brake = self.apply_brake diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 56c981326..63189bcd8 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -6,7 +6,16 @@ def create_buttons(packer, bus, idx, button): values = { "ACCButtons": button, "RollingCounter": idx, + "ACCAlwaysOne": 1, + "DistanceButton": 0, } + + checksum = 240 + int(values["ACCAlwaysOne"] * 0xf) + checksum += values["RollingCounter"] * (0x4ef if values["ACCAlwaysOne"] != 0 else 0x3f0) + checksum -= int(values["ACCButtons"] - 1) << 4 # not correct if value is 0 + checksum -= 2 * values["DistanceButton"] + + values["SteeringButtonChecksum"] = checksum return packer.make_can_msg("ASCMSteeringButton", bus, values) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 790a33f1e..cac638069 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -4,7 +4,7 @@ from math import fabs from panda import Panda from common.conversions import Conversions as CV -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 import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR from selfdrive.car.interfaces import CarInterfaceBase @@ -44,8 +44,7 @@ class CarInterface(CarInterfaceBase): return CarInterfaceBase.get_steer_feedforward_default @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False @@ -100,7 +99,7 @@ class CarInterface(CarInterfaceBase): # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is # added to selfdrive/car/tests/routes.py, we can remove it from this list. - ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL, CAR.EQUINOX, CAR.BOLT_EV} + ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL, CAR.EQUINOX} # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. # Some GMs need some tolerance above 10 kph to avoid a fault @@ -173,7 +172,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.000045 tire_stiffness_factor = 1.0 - elif candidate in (CAR.BOLT_EV, CAR.BOLT_EUV): + elif candidate == CAR.BOLT_EUV: ret.mass = 1669. + STD_CARGO_KG ret.wheelbase = 2.63779 ret.steerRatio = 16.8 @@ -197,10 +196,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.4 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 0a8cdc6db..84fa36a99 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -68,7 +68,6 @@ class CAR: ACADIA = "GMC ACADIA DENALI 2018" BUICK_REGAL = "BUICK REGAL ESSENCE 2018" ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016" - BOLT_EV = "CHEVROLET BOLT EV 2022" BOLT_EUV = "CHEVROLET BOLT EUV 2022" SILVERADO = "CHEVROLET SILVERADO 1500 2020" EQUINOX = "CHEVROLET EQUINOX 2019" @@ -101,8 +100,10 @@ CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { 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"), - CAR.BOLT_EUV: GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", "https://youtu.be/xvwzGMUA210"), + CAR.BOLT_EUV: [ + GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", "https://youtu.be/xvwzGMUA210"), + GMCarInfo("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"), + ], CAR.SILVERADO: [ GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II"), GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", "https://youtu.be/5HbNoBLzRwE"), @@ -193,9 +194,9 @@ FINGERPRINTS = { DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) -EV_CAR = {CAR.VOLT, CAR.BOLT_EV, CAR.BOLT_EUV} +EV_CAR = {CAR.VOLT, CAR.BOLT_EUV} # We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness) -CAMERA_ACC_CAR = {CAR.BOLT_EV, CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX} +CAMERA_ACC_CAR = {CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX} STEER_THRESHOLD = 1.0 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 4090d3e8d..9ea38b01a 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -189,14 +189,14 @@ class CarController: clip(CS.out.vEgo + 0.0, 0.0, 100.0), clip(CS.out.vEgo + 5.0, 0.0, 100.0)] pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V) - pcm_accel = int(1.0 * 0xc6) + pcm_accel = int(1.0 * self.params.NIDEC_GAS_MAX) else: pcm_speed_V = [0.0, clip(CS.out.vEgo - 2.0, 0.0, 100.0), clip(CS.out.vEgo + 2.0, 0.0, 100.0), clip(CS.out.vEgo + 5.0, 0.0, 100.0)] pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V) - pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6) + pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * self.params.NIDEC_GAS_MAX) if not self.CP.openpilotLongitudinalControl: if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: # radarless cars don't have supplemental message @@ -254,7 +254,7 @@ class CarController: self.speed = pcm_speed if not self.CP.enableGasInterceptor: - self.gas = pcm_accel / 0xc6 + self.gas = pcm_accel / self.params.NIDEC_GAS_MAX new_actuators = actuators.copy() new_actuators.speed = self.speed @@ -262,6 +262,7 @@ class CarController: new_actuators.gas = self.gas new_actuators.brake = self.brake new_actuators.steer = self.last_steer + new_actuators.steerOutputCan = apply_steer self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 87f8e6c5d..686119815 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -13,7 +13,8 @@ def get_pt_bus(car_fingerprint): def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False): - if radar_disabled: + no_radar = car_fingerprint in HONDA_BOSCH_RADARLESS + if radar_disabled or no_radar: # when radar is disabled, steering commands are sent directly to powertrain bus return get_pt_bus(car_fingerprint) # normally steering commands are sent to radar, which forwards them to powertrain bus @@ -104,7 +105,7 @@ def create_bosch_supplemental_1(packer, car_fingerprint): def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud): commands = [] bus_pt = get_pt_bus(CP.carFingerprint) - radar_disabled = CP.carFingerprint in HONDA_BOSCH and CP.openpilotLongitudinalControl + radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl bus_lkas = get_lkas_cmd_bus(CP.carFingerprint, radar_disabled) if CP.openpilotLongitudinalControl: @@ -153,7 +154,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, else: commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values)) - if radar_disabled and CP.carFingerprint in HONDA_BOSCH: + if radar_disabled: radar_hud_values = { 'CMBS_OFF': 0x01, 'SET_TO_1': 0x01, diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 4bfc8bce3..3241bc7ee 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -4,7 +4,7 @@ from panda import Panda from common.conversions import Conversions as CV from common.numpy_fast import interp from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS -from selfdrive.car import STD_CARGO_KG, CivicParams, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, CivicParams, create_button_event, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu from common.params import Params @@ -30,8 +30,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "honda" if candidate in HONDA_BOSCH: @@ -299,10 +298,6 @@ class CarInterface(CarInterfaceBase): stop_and_go = candidate in (HONDA_BOSCH | {CAR.CIVIC}) or ret.enableGasInterceptor ret.minEnableSpeed = -1. if stop_and_go else 25.5 * CV.MPH_TO_MS - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index dee3dc4df..86eb5fa66 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -27,6 +27,7 @@ class CarControllerParams: NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6] NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.] + NIDEC_GAS_MAX = 198 # 0xc6 NIDEC_BRAKE_MAX = 1024 // 4 BOSCH_ACCEL_MIN = -3.5 # m/s^2 @@ -243,7 +244,7 @@ FW_VERSIONS = { b'39990-TVA-A340\x00\x00', b'39990-TVA-X030\x00\x00', b'39990-TVA-X040\x00\x00', - b'39990-TVA,A150\x00\x00', + b'39990-TVA,A150\x00\x00', # modified firmware b'39990-TVE-H130\x00\x00', ], (Ecu.unknown, 0x18da3af1, None): [ diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index e5fdbfd57..5582499f2 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -60,9 +60,6 @@ class CarController: # steering torque steer = actuators.steer - if self.CP.carFingerprint in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022): - # these cars have significantly more torque than most HKG; limit to 70% of max - steer = clip(steer, -0.7, 0.7) new_steer = int(round(steer * self.params.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) @@ -86,11 +83,16 @@ class CarController: # tester present - w/ no response (keeps relevant ECU disabled) if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl: + # for longitudinal control, either radar or ADAS driving ECU addr, bus = 0x7d0, 0 if self.CP.flags & HyundaiFlags.CANFD_HDA2.value: addr, bus = 0x730, 5 can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus]) + # for blinkers + if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: + can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 5]) + # >90 degree steering fault prevention # Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault if CC.latActive and abs(CS.out.steeringAngleDeg) >= MAX_ANGLE: @@ -121,6 +123,10 @@ class CarController: if self.frame % 5 == 0 and (not hda2 or hda2_long): can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled)) + # blinkers + if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: + can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.frame, CC.leftBlinker, CC.rightBlinker)) + if self.CP.openpilotLongitudinalControl: if hda2: can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame)) @@ -189,6 +195,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = apply_steer / self.params.STEER_MAX + new_actuators.steerOutputCan = apply_steer new_actuators.accel = accel self.frame += 1 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 71507487b..0ac31a925 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -6,7 +6,7 @@ from cereal import car from common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.car.hyundai.values import HyundaiFlags, DBC, FEATURES, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams +from selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, FEATURES, CAMERA_SCC_CAR, CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams from selfdrive.car.interfaces import CarStateBase PREV_BUTTON_SAMPLES = 8 @@ -32,7 +32,6 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"] self.is_metric = False - self.brake_error = False self.buttons_counter = 0 self.cruise_info = {} @@ -72,8 +71,10 @@ class CarState(CarStateBase): self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] self.cluster_speed_counter = 0 - # mimic how dash converts to imperial - if not self.is_metric: + # Mimic how dash converts to imperial. + # Sorento is the only platform where CF_Clu_VehicleSpeed is already imperial when not is_metric + # TODO: CGW_USM1->CF_Gway_DrLockSoundRValue may describe this + if not self.is_metric and self.CP.carFingerprint not in (CAR.KIA_SORENTO,): self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH) ret.vEgoCluster = self.cluster_speed * speed_conv @@ -105,8 +106,9 @@ class CarState(CarStateBase): # TODO: Find brake pressure ret.brake = 0 ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0 - ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY + ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1 + ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED #dp ret.brakeLightsDEPRECATED = bool(cp.vl["TCS13"]["BrakeLight"] or ret.brakePressed or ret.brakeHoldActive or ret.parkingBrake) @@ -150,7 +152,6 @@ class CarState(CarStateBase): self.lkas11 = copy.copy(cp_cam.vl["LKAS11"]) self.clu11 = copy.copy(cp.vl["CLU11"]) self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE - self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED self.prev_cruise_buttons = self.cruise_buttons[-1] self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) @@ -192,7 +193,7 @@ class CarState(CarStateBase): ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1 ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"] ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"] - ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_THRESHOLD + ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5) ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"]["LEFT_LAMP"], @@ -202,7 +203,7 @@ class CarState(CarStateBase): ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0 ret.cruiseState.available = True - self.is_metric = cp.vl["CLUSTER_INFO"]["DISTANCE_UNIT"] != 1 + self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["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_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp @@ -216,6 +217,7 @@ class CarState(CarStateBase): self.cruise_buttons.extend(cp.vl_all[cruise_btn_msg]["CRUISE_BUTTONS"]) self.main_buttons.extend(cp.vl_all[cruise_btn_msg]["ADAPTIVE_CRUISE_MAIN_BTN"]) self.buttons_counter = cp.vl[cruise_btn_msg]["COUNTER"] + ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED if self.CP.flags & HyundaiFlags.CANFD_HDA2: self.cam_0x2a4 = copy.copy(cp_cam.vl["CAM_0x2a4"]) @@ -442,12 +444,12 @@ class CarState(CarStateBase): ("LKA_FAULT", "MDPS"), ("DriverBraking", "TCS"), + ("ACCEnable", "TCS"), ("COUNTER", cruise_btn_msg), ("CRUISE_BUTTONS", cruise_btn_msg), ("ADAPTIVE_CRUISE_MAIN_BTN", cruise_btn_msg), - - ("DISTANCE_UNIT", "CLUSTER_INFO"), + ("DISTANCE_UNIT", "CRUISE_BUTTONS_ALT"), ("LEFT_LAMP", "BLINKERS"), ("RIGHT_LAMP", "BLINKERS"), @@ -462,12 +464,14 @@ class CarState(CarStateBase): ("STEERING_SENSORS", 100), ("MDPS", 100), ("TCS", 50), - (cruise_btn_msg, 50), - ("CLUSTER_INFO", 4), + ("CRUISE_BUTTONS_ALT", 50), ("BLINKERS", 4), ("DOORS_SEATBELTS", 4), ] + if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS): + checks.append(("CRUISE_BUTTONS", 50)) + if CP.enableBsm: signals += [ ("FL_INDICATOR", "BLINDSPOTS_REAR_CORNERS"), diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 8b53e7c37..c492e6a5f 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -1,7 +1,6 @@ from common.numpy_fast import clip from selfdrive.car.hyundai.values import HyundaiFlags - def get_e_can_bus(CP): # On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars # have a different harness than the HDA1 and non-HDA variants in order to split @@ -98,6 +97,25 @@ def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_ove return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values) +def create_spas_messages(packer, frame, left_blink, right_blink): + ret = [] + + values = { + } + ret.append(packer.make_can_msg("SPAS1", 5, values)) + + blink = 0 + if left_blink: + blink = 3 + elif right_blink: + blink = 4 + values = { + "BLINKER_CONTROL": blink, + } + ret.append(packer.make_can_msg("SPAS2", 5, values)) + + return ret + def create_adrv_messages(packer, frame): # messages needed to car happy after disabling diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 42312ee46..0545c75eb 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -2,9 +2,9 @@ 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, CANFD_RADAR_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 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 import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.disable_ecu import disable_ecu from common.params import Params @@ -19,13 +19,7 @@ BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: Bu class CarInterface(CarInterfaceBase): @staticmethod - def get_pid_accel_limits(CP, current_speed, cruise_speed): - return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX - - @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "hyundai" ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1] or DBC[ret.carFingerprint]["radar"] is None @@ -121,8 +115,8 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.67 ret.steerRatio = 14.00 * 1.15 tire_stiffness_factor = 0.385 - elif candidate == CAR.TUCSON_HYBRID_4TH_GEN: - ret.mass = 1680. + STD_CARGO_KG # average of all 3 trims + elif candidate in (CAR.TUCSON_4TH_GEN, CAR.TUCSON_HYBRID_4TH_GEN): + ret.mass = 1630. + STD_CARGO_KG # average ret.wheelbase = 2.756 ret.steerRatio = 16. tire_stiffness_factor = 0.385 @@ -192,8 +186,16 @@ class CarInterface(CarInterfaceBase): ret.mass = 1767. + STD_CARGO_KG # SX Prestige trim support only ret.wheelbase = 2.756 ret.steerRatio = 13.6 + elif candidate == CAR.KIA_SORENTO_PHEV_4TH_GEN: + ret.mass = 4095.8 * CV.LB_TO_KG + STD_CARGO_KG # weight from EX and above trims, average of FWD and AWD versions (EX, X-Line EX AWD, SX, SX Pestige, X-Line SX Prestige AWD) + ret.wheelbase = 2.81 + ret.steerRatio = 13.27 # steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sorento-phev/2022/specifications # Genesis + elif candidate == CAR.GENESIS_GV60_EV_1ST_GEN: + ret.mass = 2205 + STD_CARGO_KG + ret.wheelbase = 2.9 + ret.steerRatio = 12.6 # https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1. elif candidate == CAR.GENESIS_G70: ret.steerActuatorDelay = 0.1 ret.mass = 1640.0 + STD_CARGO_KG @@ -273,11 +275,11 @@ class CarInterface(CarInterfaceBase): elif candidate in EV_CAR: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_EV_GAS - ret.centerToFront = ret.wheelbase * 0.4 + if candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022): + ret.flags |= HyundaiFlags.ALT_LIMITS.value + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_ALT_LIMITS - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + ret.centerToFront = ret.wheelbase * 0.4 # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors @@ -297,6 +299,10 @@ class CarInterface(CarInterfaceBase): addr, bus = 0x730, 5 disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01') + # for blinkers + if CP.flags & HyundaiFlags.ENABLE_BLINKERS: + disable_ecu(logcan, sendcan, bus=5, addr=0x7B1, com_cont_req=b'\x28\x83\x01') + def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret) @@ -316,9 +322,6 @@ class CarInterface(CarInterfaceBase): events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable) events = self.dp_atl_warning(ret, events) - if self.CS.brake_error: - events.add(EventName.brakeUnavailable) - # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: self.low_speed_alert = True diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index ecba7b749..cdcaf629a 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -1,12 +1,12 @@ -from enum import IntFlag from dataclasses import dataclass +from enum import Enum, IntFlag from typing import Dict, List, Optional, Union from cereal import car from panda.python import uds from common.conversions import Conversions as CV from selfdrive.car import dbc_dict -from selfdrive.car.docs_definitions import CarInfo, Harness +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu @@ -39,6 +39,12 @@ class CarControllerParams: CAR.KIA_OPTIMA_H, CAR.KIA_SORENTO): self.STEER_MAX = 255 + # these cars have significantly more torque than most HKG; limit to 70% of max + elif CP.flags & HyundaiFlags.ALT_LIMITS: + self.STEER_MAX = 270 + self.STEER_DELTA_UP = 2 + self.STEER_DELTA_DOWN = 3 + # Default for most HKG else: self.STEER_MAX = 384 @@ -50,6 +56,10 @@ class HyundaiFlags(IntFlag): CANFD_ALT_GEARS = 4 CANFD_CAMERA_SCC = 8 + ALT_LIMITS = 16 + + ENABLE_BLINKERS = 32 + class CAR: # Hyundai @@ -78,6 +88,7 @@ class CAR: VELOSTER = "HYUNDAI VELOSTER 2019" SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021" IONIQ_5 = "HYUNDAI IONIQ 5 2022" + TUCSON_4TH_GEN = "HYUNDAI TUCSON 4TH GEN" TUCSON_HYBRID_4TH_GEN = "HYUNDAI TUCSON HYBRID 4TH GEN" SANTA_CRUZ_1ST_GEN = "HYUNDAI SANTA CRUZ 1ST GEN" @@ -93,6 +104,7 @@ class CAR: KIA_SELTOS = "KIA SELTOS 2021" KIA_SPORTAGE_5TH_GEN = "KIA SPORTAGE 5TH GEN" KIA_SORENTO = "KIA SORENTO GT LINE 2018" + KIA_SORENTO_PHEV_4TH_GEN = "KIA SORENTO PLUG-IN HYBRID 4TH GEN" KIA_SPORTAGE_HYBRID_5TH_GEN = "KIA SPORTAGE HYBRID 5TH GEN" KIA_STINGER = "KIA STINGER GT2 2018" KIA_STINGER_2022 = "KIA STINGER 2022" @@ -100,6 +112,7 @@ class CAR: KIA_EV6 = "KIA EV6 2022" # Genesis + GENESIS_GV60_EV_1ST_GEN = "GENESIS GV60 ELECTRIC 1ST GEN" GENESIS_G70 = "GENESIS G70 2018" GENESIS_G70_2020 = "GENESIS G70 2020" GENESIS_GV70_1ST_GEN = "GENESIS GV70 1ST GEN" @@ -107,16 +120,27 @@ class CAR: GENESIS_G90 = "GENESIS G90 2017" +class Footnote(Enum): + CANFD = CarFootnote( + "Requires a red panda, additional harness box, " + + "additional OBD-C cable, USB-A to USB-A cable, and a USB-A to USB-C OTG dongle.", + Column.MODEL, shop_footnote=True) + + @dataclass class HyundaiCarInfo(CarInfo): package: str = "Smart Cruise Control (SCC)" + def init_make(self, CP: car.CarParams): + if CP.carFingerprint in CANFD_CAR: + self.footnotes.insert(0, Footnote.CANFD) + CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { CAR.ELANTRA: [ HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_b), HyundaiCarInfo("Hyundai Elantra GT 2017-19", harness=Harness.hyundai_e), - HyundaiCarInfo("Hyundai i30 2019", harness=Harness.hyundai_e), + HyundaiCarInfo("Hyundai i30 2017-19", harness=Harness.hyundai_e), ], CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k), @@ -148,11 +172,15 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { 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-23" , "Highway Driving Assist", harness=Harness.hyundai_k), + 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_4TH_GEN: [ + HyundaiCarInfo("Hyundai Tucson 2022", harness=Harness.hyundai_n), + HyundaiCarInfo("Hyundai Tucson 2023", "All", harness=Harness.hyundai_n), + ], 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), + CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2021-22", harness=Harness.hyundai_n), # Kia CAR.KIA_FORTE: HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g), @@ -175,11 +203,12 @@ 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_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", 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_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", "Smart Cruise Control (SCC)", harness=Harness.hyundai_a), 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), @@ -190,6 +219,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { ], # Genesis + CAR.GENESIS_GV60_EV_1ST_GEN: HyundaiCarInfo("Genesis GV60 2023", "All", harness=Harness.hyundai_k), 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), @@ -685,12 +715,14 @@ FW_VERSIONS = { b'\xf1\x8758910-S1DA0\xf1\x00TM ESC \x1e 102 \x08\x08 58910-S1DA0', b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0', b'\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0', + b'\xf1\x00TM ESC \x04 101 \x08\x04 58910-S2GA0', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x82TACVN5GMI3XXXH0A', b'\xf1\x82TMBZN5TMD3XXXG2E', b'\xf1\x82TACVN5GSI3XXXH0A', b'\xf1\x82TMCFD5MMCXXXXG0A', + b'\xf1\x81HM6M1_0a0_G20', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82TMDWN5TMD3TXXJ1A', b'\xf1\x81HM6M2_0a0_G00', b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_J10', @@ -715,6 +747,7 @@ FW_VERSIONS = { b'\xf1\x00HT6TA290BLHT6TAF00A1STM0M25GS1\x00\x00\x00\x00\x00\x006\xd8\x97\x15', b'\xf1\x00T02601BL T02900A1 VTMPT25XXX900NS8\xb7\xaa\xfe\xfc', b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02900A1 \xf1\x00T02601BL T02900A1 VTMPT25XXX900NS8\xb7\xaa\xfe\xfc', + b'\xf1\x00T02601BL T02800A1 VTMPT25XXX800NS4\xed\xaf\xed\xf5', ], }, CAR.SANTA_FE_HEV_2022: { @@ -819,6 +852,7 @@ FW_VERSIONS = { b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 ', b'\xf1\x00LX2_ SCC FHCUP 1.00 1.05 99110-S8100 ', b'\xf1\x00ON__ FCA FHCUP 1.00 1.02 99110-S9100 ', + b'\xf1\x00ON__ FCA FHCUP 1.00 1.01 99110-S9110 ', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360', @@ -832,6 +866,7 @@ FW_VERSIONS = { b'\xf1\x00ON ESC \x0b 100\x18\x12\x18 58910-S9360', b'\xf1\x00ON ESC \x0b 101\x19\t\x08 58910-S9360', b'\xf1\x00ON ESC \x0b 101\x19\t\x05 58910-S9320', + b'\xf1\x00ON ESC \x01 101\x19\t\x08 58910-S9360', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00', @@ -854,11 +889,13 @@ FW_VERSIONS = { b'\xf1\x00ON MFC AT USA LHD 1.00 1.01 99211-S9100 181105', b'\xf1\x00ON MFC AT USA LHD 1.00 1.03 99211-S9100 200720', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.00 99211-S8110 210226', + b'\xf1\x00ON MFC AT USA LHD 1.00 1.04 99211-S9100 211227', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', b'\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', + b'\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00TON2G38NB5j\x94.\xde', b'\xf1\x87LBLUFN591307KF25vgvw\x97wwwy\x99\xa7\x99\x99\xaa\xa9\x9af\x88\x96h\x95o\xf7\xff\x99f/\xff\xe4c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB2\xd7\xc1/\xd1', b'\xf1\x87LBLUFN650868KF36\xa9\x98\x89\x88\xa8\x88\x88\x88h\x99\xa6\x89fw\x86gw\x88\x97x\xaa\x7f\xf6\xff\xbb\xbb\x8f\xff+\x82\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', b'\xf1\x87LBLUFN655162KF36\x98\x88\x88\x88\x98\x88\x88\x88x\x99\xa7\x89x\x99\xa7\x89x\x99\x97\x89g\x7f\xf7\xffwU_\xff\xe9!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', @@ -1021,11 +1058,13 @@ FW_VERSIONS = { b'\xf1\x8799110L2000\xf1\000DL3_ SCC FHCUP 1.00 1.03 99110-L2000 ', b'\xf1\x8799110L2100\xf1\x00DL3_ SCC F-CUP 1.00 1.03 99110-L2100 ', b'\xf1\x8799110L2100\xf1\x00DL3_ SCC FHCUP 1.00 1.03 99110-L2100 ', + b'\xf1\x00DL3_ SCC F-CUP 1.00 1.03 99110-L2100 ', ], (Ecu.eps, 0x7D4, None): [ b'\xf1\x8756310-L3110\xf1\000DL3 MDPS C 1.00 1.01 56310-L3110 4DLAC101', b'\xf1\x8756310-L3220\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3220 4DLAC101', b'\xf1\x8757700-L3000\xf1\x00DL3 MDPS R 1.00 1.02 57700-L3000 4DLAP102', + b'\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3220 4DLAC101', ], (Ecu.fwdCamera, 0x7C4, None): [ b'\xf1\x00DL3 MFC AT USA LHD 1.00 1.03 99210-L3000 200915', @@ -1036,6 +1075,7 @@ FW_VERSIONS = { b'\xf1\x8758910-L3200\xf1\000DL ESC \006 101 \004\002 58910-L3200', b'\xf1\x8758910-L3800\xf1\x00DL ESC \t 101 \x07\x02 58910-L3800', b'\xf1\x8758910-L3600\xf1\x00DL ESC \x03 100 \x08\x02 58910-L3600', + b'\xf1\x00DL ESC \t 100 \x06\x02 58910-L3800', ], (Ecu.engine, 0x7E0, None): [ b'\xf1\x87391212MKT0', @@ -1048,6 +1088,7 @@ FW_VERSIONS = { b'\xf1\x87SALFEA6046104GK2wvwgeTeFg\x88\x96xwwwwffvfe?\xfd\xff\x86fo\xff\x97A\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL2T16NB1ia\x0b\xb8', b'\xf1\x87SCMSAA8572454GK1\x87x\x87\x88Vf\x86hgwvwvwwgvwwgT?\xfb\xff\x97fo\xffH\xb8\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18', b'\xf1\x87954A02N300\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 WDL3T25XXX730NS2b\x1f\xb8%', + b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18', ], }, CAR.KONA_EV: { @@ -1240,18 +1281,23 @@ FW_VERSIONS = { CAR.ELANTRA: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00PD LKAS AT USA LHD 1.01 1.01 95740-G3100 A54', + b'\xf1\x00PD LKAS AT KOR LHD 1.00 1.02 95740-G3000 A51', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DPD0H16NS0e\x0e\xcd\x8e', + b'\xf1\x006U2U0_C2\x00\x006U2T0051\x00\x00DPD0D16KS0u\xce\x1fk', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00PD MDPS C 1.00 1.04 56310/G3300 4PDDC104', + b'\xf1\x00PD MDPS C 1.00 1.00 56310G3300\x00 4PDDC100', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00PD ESC \x0b 104\x18\t\x03 58920-G3350', + b'\xf1\x00PD ESC \t 104\x18\t\x03 58920-G3350', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00PD__ SCC F-CUP 1.00 1.00 96400-G3300 ', + b'\xf1\x00PD__ SCC FNCUP 1.01 1.00 96400-G3000 ', ], }, CAR.ELANTRA_2021: { @@ -1390,6 +1436,14 @@ FW_VERSIONS = { b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00' ], }, + CAR.KIA_SORENTO_PHEV_4TH_GEN: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00MQhe SCC FHCUP 1.00 1.06 99110-P4000 ', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00MQ4HMFC AT USA LHD 1.00 1.11 99210-P2000 211217', + ] + }, CAR.KIA_EV6: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 ', @@ -1413,10 +1467,19 @@ FW_VERSIONS = { b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614', ], }, + CAR.TUCSON_4TH_GEN: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.01 99211-N9240 14T', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00NX4__ 1.01 1.00 99110-N9100 ', + ], + }, 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', + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.01 99211-N9100 14A', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NX4__ 1.00 1.00 99110-N9100 ', @@ -1456,6 +1519,14 @@ FW_VERSIONS = { b'\xf1\x00JK1_ SCC FHCUP 1.00 1.02 99110-AR000 ', ], }, + CAR.GENESIS_GV60_EV_1ST_GEN: { + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU100 211215', + ], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00JW1_ RDR ----- 1.00 1.00 99110-CU000 ', + ], + }, } CHECKSUM = { @@ -1473,16 +1544,16 @@ FEATURES = { "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, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN} +CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.TUCSON_4TH_GEN, 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, CAR.KIA_SORENTO_PHEV_4TH_GEN, CAR.GENESIS_GV60_EV_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, } +CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN} # The camera does SCC on these cars, rather than the radar CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } -HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN} # these cars use a different gas signal -EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5} +HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.TUCSON_HYBRID_4TH_GEN, CAR.KIA_SPORTAGE_HYBRID_5TH_GEN, CAR.KIA_SORENTO_PHEV_4TH_GEN} # these cars use a different gas signal +EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KONA_EV_2022, CAR.KIA_EV6, CAR.IONIQ_5, CAR.GENESIS_GV60_EV_1ST_GEN} # these cars require a special panda safety mode due to missing counters and checksums in the messages LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022} @@ -1532,10 +1603,13 @@ DBC = { CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None), CAR.KIA_EV6: dbc_dict('hyundai_canfd', None), CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), + CAR.TUCSON_4TH_GEN: dbc_dict('hyundai_canfd', None), 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), + CAR.KIA_SORENTO_PHEV_4TH_GEN: dbc_dict('hyundai_canfd', None), + CAR.GENESIS_GV60_EV_1ST_GEN: dbc_dict('hyundai_canfd', None), } diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 7e379679d..7a8abc59d 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -8,10 +8,10 @@ from cereal import car from common.basedir import BASEDIR from common.conversions import Conversions as CV from common.kalman.simple_kalman import KF1D -from common.numpy_fast import interp +from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.car import apply_hysteresis, gen_empty_fingerprint -from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_deadzone +from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness +from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_center_deadzone from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel from common.params import Params @@ -93,10 +93,34 @@ class CarInterfaceBase(ABC): def get_pid_accel_limits(CP, current_speed, cruise_speed): return ACCEL_MIN, ACCEL_MAX + @classmethod + def get_params(cls, candidate: str, fingerprint: Optional[Dict[int, Dict[int, int]]] = None, car_fw: Optional[List[car.CarParams.CarFw]] = None, experimental_long: bool = False): + if fingerprint is None: + fingerprint = gen_empty_fingerprint() + + if car_fw is None: + car_fw = list() + + ret = CarInterfaceBase.get_std_params(candidate) + ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long) + + # Set common params using fields set by the car interface + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: some car interfaces set stiffness factor + if ret.tireStiffnessFront == 0 or ret.tireStiffnessRear == 0: + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + + return ret + @staticmethod @abstractmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - pass + def _get_params(ret: car.CarParams, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool): + raise NotImplementedError @staticmethod def init(CP, logcan, sendcan): @@ -115,7 +139,7 @@ class CarInterfaceBase(ABC): def torque_from_lateral_accel_linear(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, friction_compensation): # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction) friction_interp = interp( - apply_deadzone(lateral_accel_error, lateral_accel_deadzone), + apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone), [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-torque_params.friction, torque_params.friction] ) @@ -127,7 +151,7 @@ class CarInterfaceBase(ABC): # returns a set of default params to avoid repetition in car specific params @staticmethod - def get_std_params(candidate, fingerprint): + def get_std_params(candidate): ret = car.CarParams.new_message() ret.carFingerprint = candidate @@ -349,6 +373,7 @@ class CarStateBase(ABC): self.cruise_buttons = 0 self.left_blinker_cnt = 0 self.right_blinker_cnt = 0 + self.steering_pressed_cnt = 0 self.left_blinker_prev = False self.right_blinker_prev = False self.cluster_speed_hyst_gap = 0.0 @@ -386,6 +411,12 @@ class CarStateBase(ABC): self.right_blinker_cnt = blinker_time if right_blinker_lamp else max(self.right_blinker_cnt - 1, 0) return self.left_blinker_cnt > 0, self.right_blinker_cnt > 0 + def update_steering_pressed(self, steering_pressed, steering_pressed_min_count): + """Applies filtering on steering pressed for noisy driver torque signals.""" + self.steering_pressed_cnt += 1 if steering_pressed else -1 + self.steering_pressed_cnt = clip(self.steering_pressed_cnt, 0, steering_pressed_min_count * 2) + return self.steering_pressed_cnt > steering_pressed_min_count + def update_blinker_from_stalk(self, blinker_time: int, left_blinker_stalk: bool, right_blinker_stalk: bool): """Update blinkers from stalk position. When stalk is seen the blinker will be on for at least blinker_time, or until the stalk is turned off, whichever is longer. If the opposite stalk direction is seen the blinker diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index 4b4bdcc0c..d9c658a14 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -137,17 +137,17 @@ class IsoTpParallelQuery: else: response_timeouts[tx_addr] = 0 request_done[tx_addr] = True - cloudlog.warning(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}") + cloudlog.error(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}") cur_time = time.monotonic() if cur_time - max(response_timeouts.values()) > 0: for tx_addr in msgs: if request_counter[tx_addr] > 0 and not request_done[tx_addr]: - cloudlog.warning(f"iso-tp query timeout after receiving response: {tx_addr}") + cloudlog.error(f"iso-tp query timeout after receiving response: {tx_addr}") break if cur_time - start_time > total_timeout: - cloudlog.warning("iso-tp query timeout while receiving data") + cloudlog.error("iso-tp query timeout while receiving data") break return results diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 2add59ccb..027822cc3 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -59,6 +59,7 @@ class CarController: new_actuators = CC.actuators.copy() new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + new_actuators.steerOutputCan = apply_steer self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 3f9876e90..ff3cafed3 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -2,7 +2,7 @@ from cereal import car from common.conversions import Conversions as CV from selfdrive.car.mazda.values import CAR, LKAS_LIMITS -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from common.params import Params @@ -12,9 +12,7 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "mazda" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarOffCan = True @@ -51,10 +49,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.41 - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 129273efe..e6e9b3aee 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -42,7 +42,7 @@ CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = { CAR.CX9: MazdaCarInfo("Mazda CX-9 2016-20"), CAR.MAZDA3: MazdaCarInfo("Mazda 3 2017-18"), CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"), - CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-22", video_link="https://youtu.be/dA3duO4a0O4"), + CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4"), CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-23"), } @@ -283,6 +283,7 @@ FW_VERSIONS = { b'TC3M-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ + b'PXGW-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXM4-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXM4-188K2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXM6-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -299,6 +300,7 @@ FW_VERSIONS = { b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GSH7-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ b'PXM4-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index a3194cd79..3ac487dbb 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -1,39 +1,28 @@ #!/usr/bin/env python3 -import math from cereal import car -from common.conversions import Conversions as CV from system.swaglog import cloudlog import cereal.messaging as messaging -from selfdrive.car import gen_empty_fingerprint, get_safety_config +from selfdrive.car import get_safety_config from selfdrive.car.interfaces import CarInterfaceBase + # mocked car interface to work with chffrplus -TS = 0.01 # 100Hz -YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter -# low pass gain -LPG = 2 * math.pi * YAW_FR * TS / (1 + 2 * math.pi * YAW_FR * TS) - - class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) cloudlog.debug("Using Mock Car Interface") - self.sm = messaging.SubMaster(['gyroscope', 'gpsLocation', 'gpsLocationExternal']) + self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) self.speed = 0. self.prev_speed = 0. - self.yaw_rate = 0. - self.yaw_rate_meas = 0. @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "mock" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput)] ret.mass = 1700. - ret.rotationalInertia = 2500. ret.wheelbase = 2.70 ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 13. # reasonable @@ -45,11 +34,6 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): self.sm.update(0) - - # get basic data from phone and gps since CAN isn't connected - if self.sm.updated['gyroscope']: - self.yaw_rate_meas = -self.sm['gyroscope'].gyroUncalibrated.v[0] - gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' if self.sm.updated[gps_sock]: self.prev_speed = self.speed @@ -61,10 +45,9 @@ class CarInterface(CarInterfaceBase): # speeds ret.vEgo = self.speed ret.vEgoRaw = self.speed - a = self.speed - self.prev_speed - ret.aEgo = a - ret.brakePressed = a < -0.5 + ret.aEgo = self.speed - self.prev_speed + ret.brakePressed = ret.aEgo < -0.5 ret.standstill = self.speed < 0.01 ret.wheelSpeeds.fl = self.speed @@ -72,10 +55,6 @@ class CarInterface(CarInterfaceBase): ret.wheelSpeeds.rl = self.speed ret.wheelSpeeds.rr = self.speed - self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate - curvature = self.yaw_rate / max(self.speed, 1.) - ret.steeringAngleDeg = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG - return ret def apply(self, c): diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index dbc2b33c6..ff1381239 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -1,6 +1,6 @@ from cereal import car -from common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_angle_limits from selfdrive.car.nissan import nissancan from selfdrive.car.nissan.values import CAR, CarControllerParams @@ -14,7 +14,7 @@ class CarController: self.frame = 0 self.lkas_max_torque = 0 - self.last_angle = 0 + self.apply_angle_last = 0 self.packer = CANPacker(dbc_name) @@ -28,18 +28,11 @@ class CarController: ### STEER ### lkas_hud_msg = CS.lkas_hud_msg lkas_hud_info_msg = CS.lkas_hud_info_msg - apply_angle = actuators.steeringAngleDeg - steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 if CC.latActive: # windup slower - if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): - angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) - else: - angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_VU) - - apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) + apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams) # Max torque from driver before EPS will give up and not apply torque if not bool(CS.out.steeringPressed): @@ -57,7 +50,7 @@ class CarController: apply_angle = CS.out.steeringAngleDeg self.lkas_max_torque = 0 - self.last_angle = apply_angle + self.apply_angle_last = apply_angle if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd: can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg)) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index e289b8a70..b20210ce3 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.nissan.values import CAR @@ -8,9 +8,7 @@ from selfdrive.car.nissan.values import CAR class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "nissan" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] ret.autoResumeSng = False @@ -20,6 +18,9 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.1 ret.steerRatio = 17 + ret.steerControlType = car.CarParams.SteerControlType.angle + ret.radarOffCan = True + if candidate in (CAR.ROGUE, CAR.XTRAIL): ret.mass = 1610 + STD_CARGO_KG ret.wheelbase = 2.705 @@ -37,17 +38,6 @@ class CarInterface(CarInterfaceBase): CarInterfaceBase.configure_dp_tune(candidate, ret.lateralTuning) - ret.steerControlType = car.CarParams.SteerControlType.angle - ret.radarOffCan = True - - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by - # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - return ret # returns a car.CarState diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 09bd7ca83..a6ee27a4a 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -4,7 +4,7 @@ from enum import Enum from cereal import car from panda.python import uds -from selfdrive.car import dbc_dict +from selfdrive.car import AngleRateLimit, dbc_dict from selfdrive.car.docs_definitions import CarInfo, Harness from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries @@ -12,9 +12,8 @@ Ecu = car.CarParams.Ecu class CarControllerParams: - ANGLE_DELTA_BP = [0., 5., 15.] - ANGLE_DELTA_V = [5., .8, .15] # windup limit - ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4]) LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower STEER_THRESHOLD = 1.0 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index b5429daef..a56e63408 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -87,6 +87,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 2f5eda050..1615f7bae 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from panda import Panda -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS from common.params import Params @@ -10,9 +10,7 @@ from common.params import Params class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "subaru" ret.radarOffCan = True ret.dashcamOnly = candidate in PREGLOBAL_CARS @@ -105,15 +103,6 @@ class CarInterface(CarInterfaceBase): raise ValueError(f"unknown car: {candidate}") CarInterfaceBase.configure_dp_tune(candidate, ret.lateralTuning) - - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by - # mass and CG position, so all cars will have approximately similar dyn behaviors - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - Params().put("dp_lateral_steer_rate_cost", "0.7") return ret diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 9975e495d..6ac2637fa 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -63,7 +63,7 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { ], CAR.IMPREZA_2020: [ SubaruCarInfo("Subaru Impreza 2020-22"), - SubaruCarInfo("Subaru Crosstrek 2020-21"), + SubaruCarInfo("Subaru Crosstrek 2020-23"), SubaruCarInfo("Subaru XV 2020-21"), ], CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"), @@ -235,12 +235,14 @@ FW_VERSIONS = { b'\x9a\xc0\000\000', b'\n\xc0\004\000', b'\x9a\xc0\x04\x00', + b'\n\xc0\x04\x01', ], (Ecu.fwdCamera, 0x787, None): [ b'\000\000eb\037@ \"', b'\000\000e\x8f\037@ )', b'\x00\x00eq\x1f@ "', b'\x00\x00eq\x00\x00\x00\x00', + b'\x00\x00e\x8f\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\xca!ap\a', @@ -250,6 +252,7 @@ FW_VERSIONS = { b'\xcc!fp\a', b'\xca!f@\x07', b'\xca!fp\x07', + b'\xf3"f@\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\xe6\xf5\004\000\000', @@ -258,6 +261,7 @@ FW_VERSIONS = { b'\xe7\xf5D0\000', b'\xf1\x00\xd7\x10@', b'\xe6\xf5D0\x00', + b'\xe9\xf6F0\x00', ], }, CAR.FORESTER: { diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index cf43b8ef0..6e2869d1c 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -1,5 +1,6 @@ -from common.numpy_fast import clip, interp +from common.numpy_fast import clip from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_angle_limits from selfdrive.car.tesla.teslacan import TeslaCAN from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams @@ -8,7 +9,7 @@ class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP self.frame = 0 - self.last_angle = 0 + self.apply_angle_last = 0 self.packer = CANPacker(dbc_name) self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) @@ -24,20 +25,15 @@ class CarController: lkas_enabled = CC.latActive and not hands_on_fault if lkas_enabled: - apply_angle = actuators.steeringAngleDeg - # Angular rate limit based on speed - steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle) - rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN - max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) - apply_angle = clip(apply_angle, self.last_angle - max_angle_diff, self.last_angle + max_angle_diff) + apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams) # To not fault the EPS apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20) else: apply_angle = CS.out.steeringAngleDeg - self.last_angle = apply_angle + self.apply_angle_last = apply_angle can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame)) # Longitudinal control (in sync with stock message, about 40Hz) diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 2eb29efb4..49e06d892 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -2,14 +2,13 @@ from cereal import car from panda import Panda from selfdrive.car.tesla.values import CANBUS, CAR -from selfdrive.car import STD_CARGO_KG, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_safety_config +from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "tesla" # There is no safe way to do steer blending with user torque, @@ -51,9 +50,6 @@ class CarInterface(CarInterfaceBase): else: raise ValueError(f"Unsupported car: {candidate}") - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - return ret def _update(self, c): diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 750fe885e..52f2aedf9 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -2,14 +2,13 @@ from collections import namedtuple from typing import Dict, List, Union from cereal import car -from selfdrive.car import dbc_dict +from selfdrive.car import AngleRateLimit, dbc_dict from selfdrive.car.docs_definitions import CarInfo from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) -AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) class CAR: @@ -104,8 +103,8 @@ BUTTONS = [ ] class CarControllerParams: - RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) - RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4]) JERK_LIMIT_MAX = 8 JERK_LIMIT_MIN = -8 ACCEL_TO_SPEED_MULTIPLIER = 3 diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 8d2217367..48d85584b 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -31,6 +31,8 @@ class TestCarInterfaces(unittest.TestCase): assert car_interface self.assertGreater(car_params.mass, 1) + self.assertGreater(car_params.wheelbase, 0) + self.assertGreater(car_params.centerToFront, 0) self.assertGreater(car_params.maxLateralAccel, 0) if car_params.steerControlType != car.CarParams.SteerControlType.angle: diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 6ec782444..d7b2ec407 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -34,6 +34,8 @@ 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] +KIA SORENTO PLUG-IN HYBRID 4TH GEN: [2.5, 2.5, 0.1] +GENESIS GV60 ELECTRIC 1ST GEN: [2.5, 2.5, 0.1] # Dashcam or fallback configured as ideal car mock: [10.0, 10, 0.0] diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index a9023b4ed..eb1a04cee 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -27,10 +27,11 @@ 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 5 2022: [3.172929, 2.713050, 0.096019] HYUNDAI IONIQ ELECTRIC LIMITED 2019: [1.7662975472852054, 1.613755614526594, 0.17087579756306276] HYUNDAI IONIQ PHEV 2020: [3.2928700076638537, 2.1193482926455656, 0.12463700961468778] HYUNDAI IONIQ PLUG-IN HYBRID 2019: [2.970807902012267, 1.6312321830002083, 0.1088964990357482] -HYUNDAI KONA ELECTRIC 2019: [4.398306735170212, 3.2961956260770484, 0.08651833437845884] +HYUNDAI KONA ELECTRIC 2019: [3.078814714619148, 3.2961956260770484, 0.12359762054065548] HYUNDAI PALISADE 2020: [2.544642494803999, 1.8721703683337008, 0.1301424599248651] HYUNDAI SANTA FE 2019: [3.0787027729757632, 2.6173437483495565, 0.1207019341823945] HYUNDAI SANTA FE HYBRID 2022: [3.501877602644835, 2.729064118456137, 0.10384068104538963] @@ -41,7 +42,7 @@ HYUNDAI SONATA HYBRID 2021: [2.8990264092395734, 2.061410192222139, 0.0899805488 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 EV6 2022: [3.2, 2.093457, 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] @@ -77,7 +78,7 @@ TOYOTA HIGHLANDER 2020: [2.022340166827233, 1.6183134804881791, 0.14592306380054 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: [1.746445, 1.5023147650693636, 0.151515] +TOYOTA PRIUS 2017: [1.60, 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.331293, 2.0993589721530252, 0.129822] diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index 77236e393..aeb2e6f28 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -29,13 +29,13 @@ HYUNDAI VELOSTER 2019: HYUNDAI SONATA 2019 HYUNDAI KONA 2020: HYUNDAI KONA ELECTRIC 2019 HYUNDAI KONA HYBRID 2020: HYUNDAI KONA ELECTRIC 2019 HYUNDAI KONA ELECTRIC 2022: HYUNDAI KONA ELECTRIC 2019 -HYUNDAI IONIQ 5 2022: KIA EV6 2022 HYUNDAI IONIQ HYBRID 2017-2019: HYUNDAI IONIQ PLUG-IN HYBRID 2019 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 TUCSON 2019: HYUNDAI SANTA FE 2019 +HYUNDAI TUCSON 4TH GEN: HYUNDAI TUCSON HYBRID 4TH GEN HYUNDAI SANTA FE 2022: HYUNDAI SANTA FE HYBRID 2022 KIA STINGER 2022: KIA STINGER GT2 2018 GENESIS G90 2017: GENESIS G70 2018 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index a7e779e5d..f8a81c511 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -184,7 +184,7 @@ class CarController: if self.frame % 100 == 0 or send_ui: can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, - hud_control.rightLaneDepart, CC.enabled)) + hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud)) if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu: can_sends.append(create_fcw_command(self.packer, fcw_alert)) @@ -196,6 +196,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + new_actuators.steerOutputCan = apply_steer new_actuators.accel = self.accel new_actuators.gas = self.gas diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 8c3133340..230c23dca 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,3 +1,5 @@ +import copy + from cereal import car from common.conversions import Conversions as CV from common.numpy_fast import mean @@ -6,7 +8,7 @@ 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, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR +from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR from common.params import Params, put_nonblocking import time @@ -43,6 +45,7 @@ class CarState(CarStateBase): self.low_speed_lockout = False self.acc_type = 1 + self.lkas_hud = {} #dp self.dp_sig_check = False @@ -193,16 +196,20 @@ class CarState(CarStateBase): ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale # we could use the override bit from dbc, but it's triggered at too high torque values ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - # steer rate fault, goes to 21 or 25 for 1 frame, then 9 for ~2 seconds - ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in (0, 9, 21, 25) + # steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds + # lka msg drop out: goes to 9 then 11 for a combined total of 2 seconds + ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in (0, 9, 11, 21, 25) # 17 is a fault from a prolonged high torque delta between cmd and user - ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] == 17 + # 3 is a fault from the lka command message not being received by the EPS + ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] in (3, 17) if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: + # TODO: find the bit likely in DSU_CRUISE that describes an ACC fault. one may also exist in CLUTCH ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0 ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS cluster_set_speed = cp.vl["PCM_CRUISE_ALT"]["UI_SET_SPEED"] else: + ret.accFaulted = cp.vl["PCM_CRUISE_2"]["ACC_FAULTED"] != 0 ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0 ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS cluster_set_speed = cp.vl["PCM_CRUISE_SM"]["UI_SET_SPEED"] @@ -228,7 +235,9 @@ class CarState(CarStateBase): self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2 self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] - ret.cruiseState.standstill = self.pcm_acc_status == 7 + if self.CP.carFingerprint not in (NO_STOP_TIMER_CAR - TSS2_CAR): + # ignore standstill state in certain vehicles, since pcm allows to restart with just an acceleration request + 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) @@ -242,6 +251,9 @@ class CarState(CarStateBase): ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1) ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1) + if self.CP.carFingerprint != CAR.PRIUS_V: + self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) + ret.cruiseActualEnabled = ret.cruiseState.enabled self._update_traffic_signals(cp_cam) ret.cruiseState.speedLimit = self._calculate_speed_limit() @@ -411,6 +423,7 @@ class CarState(CarStateBase): else: signals.append(("MAIN_ON", "PCM_CRUISE_2")) signals.append(("SET_SPEED", "PCM_CRUISE_2")) + signals.append(("ACC_FAULTED", "PCM_CRUISE_2")) signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2")) checks.append(("PCM_CRUISE_2", 33)) @@ -477,6 +490,18 @@ class CarState(CarStateBase): ] + if CP.carFingerprint != CAR.PRIUS_V: + signals += [ + ("LANE_SWAY_FLD", "LKAS_HUD"), + ("LANE_SWAY_BUZZER", "LKAS_HUD"), + ("LANE_SWAY_WARNING", "LKAS_HUD"), + ("LANE_SWAY_SENSITIVITY", "LKAS_HUD"), + ("LANE_SWAY_TOGGLE", "LKAS_HUD"), + ] + checks += [ + ("LKAS_HUD", 1), + ] + if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): signals += [ ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index d87aa57e3..5d91a5188 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -3,7 +3,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, 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 import STD_CARGO_KG, scale_tire_stiffness, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from common.params import Params @@ -25,9 +25,7 @@ class CarInterface(CarInterfaceBase): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], experimental_long=False): # pylint: disable=dangerous-default-value - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "toyota" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] @@ -52,12 +50,14 @@ class CarInterface(CarInterfaceBase): ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG # Only give steer angle deadzone to for bad angle sensor prius if params.get_bool("dp_toyota_prius_bad_angle_tune"): - steering_angle_deadzone_deg = 1.0 + steering_angle_deadzone_deg = 0.2 + ret.steerActuatorDelay = 0.25 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) else: for fw in car_fw: if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00': - steering_angle_deadzone_deg = 1.0 + steering_angle_deadzone_deg = 0.2 + ret.steerActuatorDelay = 0.25 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) elif candidate == CAR.PRIUS_V: @@ -66,7 +66,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.4 tire_stiffness_factor = 0.5533 ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -207,10 +206,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) - # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, @@ -261,15 +256,19 @@ class CarInterface(CarInterfaceBase): #okay ish #tune.kiBP = [0., 11., 17., 20., 24., 30., 33., 40.] #tune.kiV = [.001, .21, .22, .23, .22, .1, .001, .0001] - tune.kiBP = [0., 6., 8., 11., 30., 33., 40.] - tune.kiV = [.001, .07, .15, .2, .2, .01, .0001] + tune.kiBP = [0., 5.6, 11.1, 19.4, 30., 33., 40.] + tune.kiV = [.1, .127, .185, .185, .15, .09, .01] if candidate in TSS2_CAR: - ret.vEgoStopping = 0.2 # car is near 0.1 to 0.2 when car starts requesting stopping accel - ret.vEgoStarting = 0.2 # needs to be > or == vEgoStopping - ret.stopAccel = -2.0 # Toyota requests -0.4 when stopped - ret.stoppingDecelRate = 0.3 # reach stopping target smoothly - seems to take 0.5 seconds to go from 0 to -0.4 - ret.longitudinalActuatorDelayLowerBound = 0.3 - ret.longitudinalActuatorDelayUpperBound = 0.3 + #ret.vEgoStopping = 0.3 # car is near 0.1 to 0.2 when car starts requesting stopping accel + ret.vEgoStarting = 0.6 # needs to be > or == vEgoStopping + #ret.stopAccel = -0.1 # Toyota requests -0.4 when stopped + ret.stoppingDecelRate = 0.009 # reach stopping target smoothly - seems to take 0.5 seconds to go from 0 to -0.4 + #ret.longitudinalActuatorDelayLowerBound = 0.3 + #ret.longitudinalActuatorDelayUpperBound = 0.3 + ### stock ### + #ret.vEgoStopping = 0.25 + #ret.vEgoStarting = 0.25 + #ret.stoppingDecelRate = 0.3 # reach stopping target smoothly else: tune.kpBP = [0., 5., 35.] tune.kiBP = [0., 35.] diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 7c0fe2654..742a6aadc 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -66,13 +66,13 @@ def create_fcw_command(packer, fcw): return packer.make_can_msg("ACC_HUD", 0, values) -def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled): +def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud): values = { "TWO_BEEPS": chime, "LDA_ALERT": steer, "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2, "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2, - "BARRIERS" : 1 if enabled else 0, + "BARRIERS": 1 if enabled else 0, # static signals "SET_ME_X02": 2, @@ -96,4 +96,9 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep "ADJUSTING_CAMERA": 0, "LDW_EXIST": 1, } + + # lane sway functionality + # not all cars have LKAS_HUD — update with camera values if available + values.update(stock_lkas_hud) + return packer.make_can_msg("LKAS_HUD", 0, values) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 244b29bf9..92bef2bf2 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -114,7 +114,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { CAR.CAMRY: ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), CAR.CAMRYH: ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), CAR.CAMRY_TSS2: ToyotaCarInfo("Toyota Camry 2021-22", footnotes=[Footnote.CAMRY]), - CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-22"), + CAR.CAMRYH_TSS2: ToyotaCarInfo("Toyota Camry Hybrid 2021-23"), CAR.CHR: ToyotaCarInfo("Toyota C-HR 2017-21"), CAR.CHRH: ToyotaCarInfo("Toyota C-HR Hybrid 2017-19"), CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"), @@ -553,10 +553,12 @@ FW_VERSIONS = { (Ecu.abs, 0x7b0, None): [ b'F152633D00\x00\x00\x00\x00\x00\x00', b'F152633D60\x00\x00\x00\x00\x00\x00', + b'F152633310\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x018966306Q6000\x00\x00\x00\x00', b'\x018966306Q7000\x00\x00\x00\x00', + b'\x018966306V1000\x00\x00\x00\x00', b'\x01896633T20000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 15): [ @@ -567,6 +569,7 @@ FW_VERSIONS = { b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F3305300\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', + b'\x028646F3305500\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, CAR.CHR: { @@ -740,6 +743,7 @@ FW_VERSIONS = { b'\x018966312Q8000\x00\x00\x00\x00', b'\x018966312R0000\x00\x00\x00\x00', b'\x018966312R0100\x00\x00\x00\x00', + b'\x018966312R0200\x00\x00\x00\x00', b'\x018966312R1000\x00\x00\x00\x00', b'\x018966312R1100\x00\x00\x00\x00', b'\x018966312R3100\x00\x00\x00\x00', @@ -747,6 +751,7 @@ FW_VERSIONS = { b'\x018966312S7000\x00\x00\x00\x00', b'\x018966312W3000\x00\x00\x00\x00', b'\x018966312W9000\x00\x00\x00\x00', + b'\x01896637644000\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\x0230A10000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -774,6 +779,7 @@ FW_VERSIONS = { b'\x018965B1255000\x00\x00\x00\x00', b'8965B12361\x00\x00\x00\x00\x00\x00', b'8965B16011\x00\x00\x00\x00\x00\x00', + b'8965B76012\x00\x00\x00\x00\x00\x00', b'\x018965B12510\x00\x00\x00\x00\x00\x00', b'\x018965B1256000\x00\x00\x00\x00', ], @@ -791,6 +797,7 @@ FW_VERSIONS = { b'\x01F152612B60\x00\x00\x00\x00\x00\x00', b'\x01F152612B61\x00\x00\x00\x00\x00\x00', b'\x01F152612B62\x00\x00\x00\x00\x00\x00', + b'\x01F152612B70\x00\x00\x00\x00\x00\x00', b'\x01F152612B71\x00\x00\x00\x00\x00\x00', b'\x01F152612B81\x00\x00\x00\x00\x00\x00', b'\x01F152612B90\x00\x00\x00\x00\x00\x00', @@ -799,6 +806,7 @@ FW_VERSIONS = { b'\x01F152612862\x00\x00\x00\x00\x00\x00', b'\x01F152612B91\x00\x00\x00\x00\x00\x00', b'\x01F15260A070\x00\x00\x00\x00\x00\x00', + b'\x01F152676250\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', @@ -1328,6 +1336,7 @@ FW_VERSIONS = { b'\x01F15260R302\x00\x00\x00\x00\x00\x00', b'\x01F152642551\x00\x00\x00\x00\x00\x00', b'\x01F152642561\x00\x00\x00\x00\x00\x00', + b'\x01F152642601\x00\x00\x00\x00\x00\x00', b'\x01F152642700\x00\x00\x00\x00\x00\x00', b'\x01F152642701\x00\x00\x00\x00\x00\x00', b'\x01F152642710\x00\x00\x00\x00\x00\x00', @@ -1654,6 +1663,7 @@ FW_VERSIONS = { b'\x018966378B2100\x00\x00\x00\x00', b'\x018966378B3000\x00\x00\x00\x00', b'\x018966378G3000\x00\x00\x00\x00', + b'\x018966378B2000\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'\x01F152678221\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index fff554867..628962de7 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -2,6 +2,7 @@ from cereal import car from opendbc.can.packer import CANPacker from common.numpy_fast import clip from common.conversions import Conversions as CV +from common.realtime import DT_CTRL from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.volkswagen import mqbcan, pqcan from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams @@ -88,10 +89,13 @@ class CarController: CS.out.steeringPressed, hud_alert, hud_control)) if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: + lead_distance = 0 + if hud_control.leadVisible and self.frame * DT_CTRL > 1.0: # Don't display lead until we know the scaling factor + lead_distance = 512 if CS.upscale_lead_car_signal else 8 acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) set_speed = hud_control.setSpeed * CV.MS_TO_KPH # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem? can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, - hud_control.leadVisible)) + lead_distance)) # **** Stock ACC Button Controls **************************************** # @@ -103,6 +107,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"] self.frame += 1 diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 54e0693bc..1c57832e3 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -13,6 +13,7 @@ class CarState(CarStateBase): self.CCP = CarControllerParams(CP) self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} self.esp_hold_confirmation = False + self.upscale_lead_car_signal = False def create_button_events(self, pt_cp, buttons): button_events = [] @@ -143,6 +144,9 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0 + # Digital instrument clusters expect the ACC HUD lead car distance to be scaled differently + self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) + ret.cruiseActualEnabled = ret.cruiseState.enabled return ret @@ -223,12 +227,13 @@ class CarState(CarStateBase): ret.stockAeb = False # Update ACC radar status. - self.acc_type = 0 # TODO: this is ACC "basic" with nonzero min speed, support FtS (1) later + self.acc_type = ext_cp.vl["ACC_System"]["ACS_Typ_ACC"] ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"]) - ret.cruiseState.enabled = bool(pt_cp.vl["Motor_2"]["GRA_Status"]) + ret.cruiseState.enabled = pt_cp.vl["Motor_2"]["GRA_Status"] in (1, 2) if self.CP.pcmCruise: ret.accFaulted = ext_cp.vl["ACC_GRA_Anziege"]["ACA_StaACC"] in (6, 7) - # TODO: update opendbc with PQ TSK state for OP long accFaulted + else: + ret.accFaulted = pt_cp.vl["Motor_2"]["GRA_Status"] == 3 # Update ACC setpoint. When the setpoint reads as 255, the driver has not # yet established an ACC setpoint, so treat it as zero. @@ -283,6 +288,7 @@ class CarState(CarStateBase): ("ESP_Tastung_passiv", "ESP_21"), # Stability control disabled ("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation ("KBI_Handbremse", "Kombi_01"), # Manual handbrake applied + ("KBI_Variante", "Kombi_03"), # Digital/full-screen instrument cluster installed ("TSK_Status", "TSK_06"), # ACC engagement status from drivetrain coordinator ("GRA_Hauptschalter", "GRA_ACC_01"), # ACC button, on/off ("GRA_Abbrechen", "GRA_ACC_01"), # ACC button, cancel @@ -316,6 +322,7 @@ class CarState(CarStateBase): ("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) + ("Kombi_03", 0), # From J285 instrument cluster (not present on older cars, 1Hz when present) ] if CP.transmissionType == TransmissionType.automatic: @@ -490,15 +497,15 @@ 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 + ("ACC_Typ", "ACC_06"), # Basic vs FtS 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 + ("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 ] bsm_radar_signals = [ ("SWA_Infostufe_SWA_li", "SWA_01"), # Blind spot object info, left @@ -507,24 +514,26 @@ class MqbExtraSignals: ("SWA_Warnung_SWA_re", "SWA_01"), # Blind spot object warning, right ] bsm_radar_checks = [ - ("SWA_01", 20), # From J1086 Lane Change Assist + ("SWA_01", 20), # From J1086 Lane Change Assist ] class PqExtraSignals: # Additional signal and message lists for optional or bus-portable controllers fwd_radar_signals = [ - ("ACA_StaACC", "ACC_GRA_Anziege", 0), # ACC drivetrain coordinator status - ("ACA_V_Wunsch", "ACC_GRA_Anziege", 0), # ACC set speed + ("ACS_Typ_ACC", "ACC_System"), # Basic vs FtS (no SnG support on PQ) + ("ACA_StaACC", "ACC_GRA_Anziege"), # ACC drivetrain coordinator status + ("ACA_V_Wunsch", "ACC_GRA_Anziege"), # ACC set speed ] fwd_radar_checks = [ - ("ACC_GRA_Anziege", 25), # From J428 ACC radar control module + ("ACC_System", 50), # From J428 ACC radar control module + ("ACC_GRA_Anziege", 25), # From J428 ACC radar control module ] bsm_radar_signals = [ - ("SWA_Infostufe_SWA_li", "SWA_1", 0), # Blind spot object info, left - ("SWA_Warnung_SWA_li", "SWA_1", 0), # Blind spot object warning, left - ("SWA_Infostufe_SWA_re", "SWA_1", 0), # Blind spot object info, right - ("SWA_Warnung_SWA_re", "SWA_1", 0), # Blind spot object warning, right + ("SWA_Infostufe_SWA_li", "SWA_1"), # Blind spot object info, left + ("SWA_Warnung_SWA_li", "SWA_1"), # Blind spot object warning, left + ("SWA_Infostufe_SWA_re", "SWA_1"), # Blind spot object info, right + ("SWA_Warnung_SWA_re", "SWA_1"), # Blind spot object warning, right ] bsm_radar_checks = [ - ("SWA_1", 20), # From J1086 Lane Change Assist + ("SWA_1", 20), # From J1086 Lane Change Assist ] diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index bfbe8f5cd..2d2660ce5 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,8 +1,7 @@ from cereal import car from panda import Panda from common.conversions import Conversions as CV -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, \ - gen_empty_fingerprint, get_safety_config +from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter @@ -22,8 +21,7 @@ class CarInterface(CarInterfaceBase): self.cp_ext = self.cp_cam @staticmethod - def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): - ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "volkswagen" ret.radarOffCan = True @@ -74,7 +72,6 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 ret.steerRatio = 15.6 # Let the params learner figure this out - tire_stiffness_factor = 1.0 # Let the params learner figure this out ret.lateralTuning.pid.kpBP = [0.] ret.lateralTuning.pid.kiBP = [0.] ret.lateralTuning.pid.kf = 0.00006 @@ -137,7 +134,6 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.SHARAN_MK2: ret.mass = 1639 + STD_CARGO_KG ret.wheelbase = 2.92 - ret.minEnableSpeed = 30 * CV.KPH_TO_MS ret.minSteerSpeed = 50 * CV.KPH_TO_MS ret.steerActuatorDelay = 0.2 @@ -216,10 +212,7 @@ class CarInterface(CarInterfaceBase): CarInterfaceBase.configure_dp_tune(candidate, ret.lateralTuning) ret.autoResumeSng = ret.minEnableSpeed == -1 - ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) ret.centerToFront = ret.wheelbase * 0.45 - ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, - tire_stiffness_factor=tire_stiffness_factor) return ret # returns a car.CarState @@ -241,7 +234,7 @@ class CarInterface(CarInterfaceBase): events.add(EventName.belowSteerSpeed) if self.CS.CP.openpilotLongitudinalControl: - if ret.vEgo < self.CP.minEnableSpeed + 2.: + if ret.vEgo < self.CP.minEnableSpeed + 0.5: events.add(EventName.belowEngageSpeed) if c.enabled and ret.vEgo < self.CP.minEnableSpeed: events.add(EventName.speedTooLow) diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 25a710dbb..30a51f6fe 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -96,13 +96,13 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): 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 + "ACC_Abstandsindex": lead_distance, } return packer.make_can_msg("ACC_02", bus, values) diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 0bcbf6abb..130f10795 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -66,6 +66,7 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, "ACS_Sta_ADR": acc_control, "ACS_StSt_Info": acc_control != 1, "ACS_Typ_ACC": acc_type, + "ACS_Anhaltewunsch": acc_type == 1 and stopping, "ACS_Sollbeschl": accel if acc_control == 1 else 3.01, "ACS_zul_Regelabw": 0.2 if acc_control == 1 else 1.27, "ACS_max_AendGrad": 3.0 if acc_control == 1 else 5.08, @@ -76,12 +77,12 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): values = { "ACA_StaACC": acc_hud_status, "ACA_Zeitluecke": 2, "ACA_V_Wunsch": set_speed, - "ACA_gemZeitl": 8 if lead_visible else 0, + "ACA_gemZeitl": lead_distance, # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke # display/display-prio handling probably needed to stop confusing the instrument cluster # kmh_mph handling probably needed to resolve rounding errors in displayed setpoint diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index f24448adb..208d146e2 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -280,12 +280,14 @@ FW_VERSIONS = { CAR.ARTEON_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x873G0906259F \xf1\x890004', + b'\xf1\x873G0906259N \xf1\x890004', b'\xf1\x873G0906259P \xf1\x890001', b'\xf1\x875NA907115H \xf1\x890002', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158L \xf1\x893611', b'\xf1\x870GC300011L \xf1\x891401', + b'\xf1\x870GC300014M \xf1\x892802', b'\xf1\x870GC300040P \xf1\x891401', ], (Ecu.srs, 0x715, None): [ @@ -297,8 +299,10 @@ FW_VERSIONS = { b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571B41815A1', b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571B00817A1', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\00567B0020800', + b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x002MB4092M7N', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x872Q0907572T \xf1\x890383', b'\xf1\x875Q0907572J \xf1\x890654', ], @@ -316,11 +320,13 @@ FW_VERSIONS = { b'\xf1\x8703H906026J \xf1\x899971', b'\xf1\x8703H906026S \xf1\x896693', b'\xf1\x8703H906026S \xf1\x899970', + b'\xf1\x873CN906259 \xf1\x890005', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x8709G927158A \xf1\x893387', b'\xf1\x8709G927158DR\xf1\x893536', b'\xf1\x8709G927158DR\xf1\x893742', + b'\xf1\x8709G927158F \xf1\x893489', b'\xf1\x8709G927158FT\xf1\x893835', b'\xf1\x8709G927158GL\xf1\x893939', ], @@ -334,6 +340,7 @@ FW_VERSIONS = { (Ecu.eps, 0x712, None): [ b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\00571B60924A1', b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6G920A1', + b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6080105', b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6090105', ], (Ecu.fwdRadar, 0x757, None): [ @@ -373,6 +380,7 @@ FW_VERSIONS = { b'\xf1\x870EA906016Q \xf1\x895993', b'\xf1\x870EA906016S \xf1\x897207', b'\xf1\x875G0906259 \xf1\x890007', + b'\xf1\x875G0906259D \xf1\x890002', b'\xf1\x875G0906259J \xf1\x890002', b'\xf1\x875G0906259L \xf1\x890002', b'\xf1\x875G0906259N \xf1\x890003', @@ -408,6 +416,7 @@ FW_VERSIONS = { b'\xf1\x870D9300012 \xf1\x895045', b'\xf1\x870D9300014M \xf1\x895004', b'\xf1\x870D9300014Q \xf1\x895006', + b'\xf1\x870D9300020J \xf1\x894902', b'\xf1\x870D9300020Q \xf1\x895201', b'\xf1\x870D9300020S \xf1\x895201', b'\xf1\x870D9300040A \xf1\x893613', @@ -455,6 +464,7 @@ FW_VERSIONS = { b'\xf1\x873Q0909144F \xf1\x895043\xf1\x82\x0561A01612A0', b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\x0566A0J612A1', b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566A00514A1', + b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566A01613A1', b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566A0J712A1', b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571A0J714A1', b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571A0JA15A1', @@ -797,6 +807,7 @@ FW_VERSIONS = { b'\xf1\x875G0906259L \xf1\x890002', b'\xf1\x875G0906259Q \xf1\x890002', b'\xf1\x878V0906259F \xf1\x890002', + b'\xf1\x878V0906259H \xf1\x890002', b'\xf1\x878V0906259J \xf1\x890002', b'\xf1\x878V0906259K \xf1\x890001', b'\xf1\x878V0906264B \xf1\x890003', @@ -807,6 +818,7 @@ FW_VERSIONS = { b'\xf1\x870CW300044T \xf1\x895245', b'\xf1\x870CW300048 \xf1\x895201', b'\xf1\x870D9300012 \xf1\x894912', + b'\xf1\x870D9300012 \xf1\x894931', b'\xf1\x870D9300012K \xf1\x894513', b'\xf1\x870D9300013B \xf1\x894931', b'\xf1\x870D9300041N \xf1\x894512', @@ -829,12 +841,14 @@ FW_VERSIONS = { b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\x13111112111111--241115141112221291163221', b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\023111112111111--171115141112221291163221', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023121111111211--261117141112231291163221', + b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13111112111111--241115141112221291163221', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13121111111111--341117141212231291163221', b'\xf1\x875Q0959655N \xf1\x890361\xf1\x82\0211212001112110004110411111421149114', b'\xf1\x875Q0959655N \xf1\x890361\xf1\x82\0211212001112111104110411111521159114', ], (Ecu.eps, 0x712, None): [ b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566G0HA14A1', + b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566G0HA14A1', b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G01A16A1', b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G0HA16A1', b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571G0JA14A1', diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 092381152..7887b93a2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,7 +21,7 @@ from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.longcontrol import LongControl from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI -from selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD from selfdrive.controls.lib.latcontrol_torque import LatControlTorque from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR from selfdrive.controls.lib.events import Events, ET @@ -258,7 +258,7 @@ 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 not self.v_cruise_helper.v_cruise_initialized and resume_pressed: + if self.sm['dragonConf'].dpAtl == 0 and 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 @@ -267,6 +267,9 @@ class Controls: (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill))): self.events.add(EventName.pedalPressed) + if CS.brakePressed and CS.standstill: + self.events.add(EventName.preEnableStandstill) + if CS.gasPressed: self.events.add(EventName.gasPressedOverride) @@ -437,17 +440,6 @@ class Controls: if self.sm['liveLocationKalman'].excessiveResets: self.events.add(EventName.localizerMalfunction) - # Only allow engagement with brake pressed when stopped behind another stopped car - if self.sm['dragonConf'].dpAtl == 0: - speeds = self.sm['longitudinalPlan'].speeds - if len(speeds) > 1: - v_future = speeds[-1] - else: - v_future = 100.0 - if CS.brakePressed and v_future >= self.CP.vEgoStarting \ - and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3: - self.events.add(EventName.noTarget) - def data_sample(self): """Receive data from sockets and update carState""" @@ -539,10 +531,7 @@ class Controls: # PRE ENABLING elif self.state == State.preEnabled: - if self.events.any(ET.NO_ENTRY): - self.state = State.disabled - self.current_alert_types.append(ET.NO_ENTRY) - elif not self.events.any(ET.PRE_ENABLE): + if not self.events.any(ET.PRE_ENABLE): self.state = State.enabled else: self.current_alert_types.append(ET.PRE_ENABLE) @@ -605,7 +594,7 @@ class Controls: # Check which actuators can be enabled CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ CS.vEgo > self.CP.minSteerSpeed and not CS.standstill - CC.longActive = self.active and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl + CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl if self.sm['dragonConf'].dpAtl == 2 and not CS.cruiseActualEnabled: CC.longActive = False @@ -613,6 +602,11 @@ class Controls: actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state + # Enable blinkers while lane changing + if self.sm['lateralPlan'].laneChangeState != LaneChangeState.off: + CC.leftBlinker = self.sm['lateralPlan'].laneChangeDirection == LaneChangeDirection.left + CC.rightBlinker = self.sm['lateralPlan'].laneChangeDirection == LaneChangeDirection.right + if CS.leftBlinker or CS.rightBlinker: self.last_blinker_frame = self.sm.frame @@ -661,7 +655,7 @@ class Controls: max_torque = abs(self.last_actuators.steer) > 0.99 if undershooting and turning and good_speed and max_torque: self.events.add(EventName.steerSaturated) - elif lac_log.active and not CS.steeringPressed and lac_log.saturated: + elif lac_log.active and lac_log.saturated: dpath_points = lat_plan.dPathPoints if len(dpath_points): # Check if we deviated from the path @@ -762,7 +756,11 @@ class Controls: self.last_actuators, can_sends = self.CI.apply(CC) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) CC.actuatorsOutput = self.last_actuators - self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2 + if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + self.steer_limited = abs(CC.actuators.steeringAngleDeg - CC.actuatorsOutput.steeringAngleDeg) > \ + STEER_ANGLE_SATURATION_THRESHOLD + else: + self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2 force_decel = False if self.dp_jetson else (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 85b7b92cf..1ac43e2ef 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -162,6 +162,12 @@ def apply_deadzone(error, deadzone): return error +def apply_center_deadzone(error, deadzone): + if (error > - deadzone) and (error < deadzone): + error = 0. + return error + + def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index a0ce417ef..2f7642377 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -195,7 +195,7 @@ class NormalPermanentAlert(Alert): class StartupAlert(Alert): - def __init__(self, alert_text_1: str, alert_text_2: str = _("Always keep hands on wheel and eyes on road"), alert_status=AlertStatus.normal): + def __init__(self, alert_text_1: str, alert_text_2: str = _("Happy Holiday / New Years! Safe Travel, cya in 2023"), alert_status=AlertStatus.normal): super().__init__(alert_text_1, alert_text_2, alert_status, AlertSize.mid, Priority.LOWER, VisualAlert.none, AudibleAlert.none, 10.), @@ -514,9 +514,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { EventName.resumeRequired: { ET.WARNING: Alert( - _("STOPPED"), _("Press Resume to Exit Standstill"), - AlertStatus.userPrompt, AlertSize.mid, + _(""), + AlertStatus.userPrompt, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), }, @@ -641,9 +641,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { visual_alert=VisualAlert.brakePressed), }, - EventName.pedalPressedPreEnable: { + EventName.preEnableStandstill: { ET.PRE_ENABLE: Alert( - _("Release Pedal to Engage"), + _("Release Brake to Engage"), "", AlertStatus.normal, AlertSize.small, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, creation_delay=1.), @@ -832,9 +832,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.accFaulted: { - ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("Cruise Faulted")), - ET.PERMANENT: NormalPermanentAlert(_("Cruise Faulted"), ""), - ET.NO_ENTRY: NoEntryAlert(_("Cruise Faulted")), + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("Cruise Fault: Restart the Car")), + ET.PERMANENT: NormalPermanentAlert(_("Cruise Fault: Restart the car to engage")), + ET.NO_ENTRY: NoEntryAlert(_("Cruise Fault: Restart the Car")), }, EventName.accFaultedTemp: { @@ -942,15 +942,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert(_("Harness Relay Malfunction")), }, - EventName.noTarget: { - ET.IMMEDIATE_DISABLE: Alert( - _("openpilot Canceled"), - _("No close lead car"), - AlertStatus.normal, AlertSize.mid, - Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.), - ET.NO_ENTRY: NoEntryAlert(_("No Close Lead Car")), - }, - EventName.speedTooLow: { ET.IMMEDIATE_DISABLE: Alert( _("openpilot Canceled"), diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 78b59fda5..cefbc1078 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -11,6 +11,7 @@ class LatControl(ABC): self.sat_count_rate = 1.0 * DT_CTRL self.sat_limit = CP.steerLimitTimer self.sat_count = 0. + self.sat_check_min_speed = 10. # we define the steer torque scale as [-1.0...1.0] self.steer_max = 1.0 @@ -23,7 +24,7 @@ class LatControl(ABC): self.sat_count = 0. def _check_saturation(self, saturated, CS, steer_limited): - if saturated and CS.vEgo > 10. and not steer_limited and not CS.steeringPressed: + if saturated and CS.vEgo > self.sat_check_min_speed and not steer_limited and not CS.steeringPressed: self.sat_count += self.sat_count_rate else: self.sat_count -= self.sat_count_rate diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 0e5be4a97..d692f80b4 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -7,6 +7,10 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): + def __init__(self, CP, CI): + super().__init__(CP, CI) + self.sat_check_min_speed = 5. + def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): angle_log = log.ControlsState.LateralAngleState.new_message() @@ -19,7 +23,7 @@ class LatControlAngle(LatControl): angle_steers_des += params.angleOffsetDeg angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD - angle_log.saturated = self._check_saturation(angle_control_saturated, CS, steer_limited) + angle_log.saturated = self._check_saturation(angle_control_saturated, CS, False) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des return 0, float(angle_steers_des), angle_log diff --git a/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json b/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json deleted file mode 100644 index af9c99f25..000000000 --- a/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json +++ /dev/null @@ -1,544 +0,0 @@ -{ - "acados_include_path": "/data/openpilot/third_party/acados/include/acados/include", - "acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib", - "code_export_directory": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code", - "constraints": { - "C": [], - "C_e": [], - "D": [], - "constr_type": "BGH", - "constr_type_e": "BGH", - "idxbu": [], - "idxbx": [ - 2, - 3 - ], - "idxbx_0": [ - 0, - 1, - 2, - 3 - ], - "idxbx_e": [], - "idxbxe_0": [ - 0, - 1, - 2, - 3 - ], - "idxsbu": [], - "idxsbx": [], - "idxsbx_e": [], - "idxsg": [], - "idxsg_e": [], - "idxsh": [], - "idxsh_e": [], - "idxsphi": [], - "idxsphi_e": [], - "lbu": [], - "lbx": [ - -1.5707963267948966, - -0.8726646259971648 - ], - "lbx_0": [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - "lbx_e": [], - "lg": [], - "lg_e": [], - "lh": [], - "lh_e": [], - "lphi": [], - "lphi_e": [], - "lsbu": [], - "lsbx": [], - "lsbx_e": [], - "lsg": [], - "lsg_e": [], - "lsh": [], - "lsh_e": [], - "lsphi": [], - "lsphi_e": [], - "ubu": [], - "ubx": [ - 1.5707963267948966, - 0.8726646259971648 - ], - "ubx_0": [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - "ubx_e": [], - "ug": [], - "ug_e": [], - "uh": [], - "uh_e": [], - "uphi": [], - "uphi_e": [], - "usbu": [], - "usbx": [], - "usbx_e": [], - "usg": [], - "usg_e": [], - "ush": [], - "ush_e": [], - "usphi": [], - "usphi_e": [] - }, - "cost": { - "Vu": [ - [ - 0.0 - ], - [ - 0.0 - ], - [ - 0.0 - ], - [ - 0.0 - ], - [ - 0.0 - ] - ], - "Vu_0": [ - [ - 0.0 - ], - [ - 0.0 - ], - [ - 0.0 - ], - [ - 0.0 - ], - [ - 0.0 - ] - ], - "Vx": [ - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_0": [ - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_e": [ - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vz": [], - "Vz_0": [], - "W": [ - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "W_0": [ - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "W_e": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ] - ], - "Zl": [], - "Zl_e": [], - "Zu": [], - "Zu_e": [], - "cost_ext_fun_type": "casadi", - "cost_ext_fun_type_0": "casadi", - "cost_ext_fun_type_e": "casadi", - "cost_type": "NONLINEAR_LS", - "cost_type_0": "NONLINEAR_LS", - "cost_type_e": "NONLINEAR_LS", - "yref": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "yref_0": [ - 0.0, - 0.0, - 0.0, - 0.0, - 0.0 - ], - "yref_e": [ - 0.0, - 0.0, - 0.0 - ], - "zl": [], - "zl_e": [], - "zu": [], - "zu_e": [] - }, - "cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include", - "dims": { - "N": 16, - "nbu": 0, - "nbx": 2, - "nbx_0": 4, - "nbx_e": 0, - "nbxe_0": 4, - "ng": 0, - "ng_e": 0, - "nh": 0, - "nh_e": 0, - "np": 2, - "nphi": 0, - "nphi_e": 0, - "nr": 0, - "nr_e": 0, - "ns": 0, - "ns_e": 0, - "nsbu": 0, - "nsbx": 0, - "nsbx_e": 0, - "nsg": 0, - "nsg_e": 0, - "nsh": 0, - "nsh_e": 0, - "nsphi": 0, - "nsphi_e": 0, - "nu": 1, - "nx": 4, - "ny": 5, - "ny_0": 5, - "ny_e": 3, - "nz": 0 - }, - "model": { - "dyn_disc_fun": null, - "dyn_disc_fun_jac": null, - "dyn_disc_fun_jac_hess": null, - "dyn_ext_fun_type": "casadi", - "dyn_source_discrete": null, - "gnsf": { - "nontrivial_f_LO": 1, - "purely_linear": 0 - }, - "name": "lat" - }, - "parameter_values": [ - 0.0, - 0.0 - ], - "problem_class": "OCP", - "simulink_opts": { - "inputs": { - "cost_W": 0, - "cost_W_0": 0, - "cost_W_e": 0, - "lbu": 1, - "lbx": 1, - "lbx_0": 1, - "lbx_e": 1, - "lg": 1, - "lh": 1, - "parameter_traj": 1, - "reset_solver": 0, - "u_init": 0, - "ubu": 1, - "ubx": 1, - "ubx_0": 1, - "ubx_e": 1, - "ug": 1, - "uh": 1, - "x_init": 0, - "y_ref": 1, - "y_ref_0": 1, - "y_ref_e": 1 - }, - "outputs": { - "CPU_time": 1, - "CPU_time_lin": 0, - "CPU_time_qp": 0, - "CPU_time_sim": 0, - "KKT_residual": 1, - "solver_status": 1, - "sqp_iter": 1, - "u0": 1, - "utraj": 0, - "x1": 1, - "xtraj": 0 - }, - "samplingtime": "t0" - }, - "solver_options": { - "Tsim": 0.009765625, - "alpha_min": 0.05, - "alpha_reduction": 0.7, - "collocation_type": "GAUSS_LEGENDRE", - "eps_sufficient_descent": 0.0001, - "exact_hess_constr": 1, - "exact_hess_cost": 1, - "exact_hess_dyn": 1, - "ext_cost_num_hess": 0, - "full_step_dual": 0, - "globalization": "FIXED_STEP", - "globalization_use_SOC": 0, - "hessian_approx": "GAUSS_NEWTON", - "hpipm_mode": "BALANCE", - "initialize_t_slacks": 0, - "integrator_type": "ERK", - "levenberg_marquardt": 0.0, - "line_search_use_sufficient_descent": 0, - "model_external_shared_lib_dir": null, - "model_external_shared_lib_name": null, - "nlp_solver_max_iter": 100, - "nlp_solver_step_length": 1.0, - "nlp_solver_tol_comp": 1e-06, - "nlp_solver_tol_eq": 1e-06, - "nlp_solver_tol_ineq": 1e-06, - "nlp_solver_tol_stat": 1e-06, - "nlp_solver_type": "SQP_RTI", - "print_level": 0, - "qp_solver": "PARTIAL_CONDENSING_HPIPM", - "qp_solver_cond_N": 1, - "qp_solver_iter_max": 1, - "qp_solver_tol_comp": null, - "qp_solver_tol_eq": null, - "qp_solver_tol_ineq": null, - "qp_solver_tol_stat": null, - "qp_solver_warm_start": 0, - "regularize_method": null, - "sim_method_jac_reuse": [ - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0 - ], - "sim_method_newton_iter": 3, - "sim_method_num_stages": [ - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4 - ], - "sim_method_num_steps": [ - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1 - ], - "tf": 2.5, - "time_steps": [ - 0.009765625, - 0.029296875, - 0.048828125, - 0.068359375, - 0.087890625, - 0.107421875, - 0.126953125, - 0.146484375, - 0.166015625, - 0.185546875, - 0.205078125, - 0.224609375, - 0.244140625, - 0.263671875, - 0.283203125, - 0.302734375 - ] - } -} \ No newline at end of file diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c deleted file mode 100644 index d0c541d3b..000000000 --- a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c +++ /dev/null @@ -1,278 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ -// standard -#include -#include - -// acados -#include "acados_c/external_function_interface.h" -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -#include "acados/sim/sim_common.h" -#include "acados/utils/external_function_generic.h" -#include "acados/utils/print.h" - - -// example specific -#include "lat_model/lat_model.h" -#include "acados_sim_solver_lat.h" - - -// ** solver data ** - -sim_solver_capsule * lat_acados_sim_solver_create_capsule() -{ - void* capsule_mem = malloc(sizeof(sim_solver_capsule)); - sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem; - - return capsule; -} - - -int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule) -{ - free(capsule); - return 0; -} - - -int lat_acados_sim_create(sim_solver_capsule * capsule) -{ - // initialize - const int nx = LAT_NX; - const int nu = LAT_NU; - const int nz = LAT_NZ; - const int np = LAT_NP; - bool tmp_bool; - - - double Tsim = 0.009765625; - - - // explicit ode - capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - - capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw; - capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in; - capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out; - capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in; - capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out; - capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work; - external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np); - - capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun; - capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in; - capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out; - capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in; - capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out; - capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work; - external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np); - - - - // sim plan & config - sim_solver_plan_t plan; - plan.sim_solver = ERK; - - // create correct config based on plan - sim_config * lat_sim_config = sim_config_create(plan); - capsule->acados_sim_config = lat_sim_config; - - // sim dims - void *lat_sim_dims = sim_dims_create(lat_sim_config); - capsule->acados_sim_dims = lat_sim_dims; - sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx); - sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu); - sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz); - - - // sim opts - sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims); - capsule->acados_sim_opts = lat_sim_opts; - int tmp_int = 3; - sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int); - sim_collocation_type collocation_type = GAUSS_LEGENDRE; - sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type); - - - tmp_int = 4; - sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int); - tmp_int = 1; - sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int); - tmp_bool = 0; - sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool); - - - // sim in / out - sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims); - capsule->acados_sim_in = lat_sim_in; - sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims); - capsule->acados_sim_out = lat_sim_out; - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "T", &Tsim); - - // model functions - lat_sim_config->model_set(lat_sim_in->model, - "expl_vde_for", capsule->sim_forw_vde_casadi); - lat_sim_config->model_set(lat_sim_in->model, - "expl_ode_fun", capsule->sim_expl_ode_fun_casadi); - - // sim solver - sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config, - lat_sim_dims, lat_sim_opts); - capsule->acados_sim_solver = lat_sim_solver; - - - /* initialize parameter values */ - double* p = calloc(np, sizeof(double)); - - - lat_acados_sim_update_params(capsule, p, np); - free(p); - - - /* initialize input */ - // x - double x0[4]; - for (int ii = 0; ii < 4; ii++) - x0[ii] = 0.0; - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "x", x0); - - - // u - double u0[1]; - for (int ii = 0; ii < 1; ii++) - u0[ii] = 0.0; - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "u", u0); - - // S_forw - double S_forw[20]; - for (int ii = 0; ii < 20; ii++) - S_forw[ii] = 0.0; - for (int ii = 0; ii < 4; ii++) - S_forw[ii + ii * 4 ] = 1.0; - - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "S_forw", S_forw); - - int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out); - - return status; -} - - -int lat_acados_sim_solve(sim_solver_capsule *capsule) -{ - // integrate dynamics using acados sim_solver - int status = sim_solve(capsule->acados_sim_solver, - capsule->acados_sim_in, capsule->acados_sim_out); - if (status != 0) - printf("error in lat_acados_sim_solve()! Exiting.\n"); - - return status; -} - - -int lat_acados_sim_free(sim_solver_capsule *capsule) -{ - // free memory - sim_solver_destroy(capsule->acados_sim_solver); - sim_in_destroy(capsule->acados_sim_in); - sim_out_destroy(capsule->acados_sim_out); - sim_opts_destroy(capsule->acados_sim_opts); - sim_dims_destroy(capsule->acados_sim_dims); - sim_config_destroy(capsule->acados_sim_config); - - // free external function - external_function_param_casadi_free(capsule->sim_forw_vde_casadi); - external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi); - - return 0; -} - - -int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np) -{ - int status = 0; - int casadi_np = LAT_NP; - - if (casadi_np != np) { - printf("lat_acados_sim_update_params: trying to set %i parameters for external functions." - " External function has %i parameters. Exiting.\n", np, casadi_np); - exit(1); - } - capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p); - capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p); - - return status; -} - -/* getters pointers to C objects*/ -sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_config; -}; - -sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_in; -}; - -sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_out; -}; - -void * lat_acados_get_sim_dims(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_dims; -}; - -sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_opts; -}; - -sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_solver; -}; - diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h deleted file mode 100644 index 86b9c84c9..000000000 --- a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#ifndef ACADOS_SIM_lat_H_ -#define ACADOS_SIM_lat_H_ - -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -#define LAT_NX 4 -#define LAT_NZ 0 -#define LAT_NU 1 -#define LAT_NP 2 - -#ifdef __cplusplus -extern "C" { -#endif - - -// ** capsule for solver data ** -typedef struct sim_solver_capsule -{ - // acados objects - sim_in *acados_sim_in; - sim_out *acados_sim_out; - sim_solver *acados_sim_solver; - sim_opts *acados_sim_opts; - sim_config *acados_sim_config; - void *acados_sim_dims; - - /* external functions */ - // ERK - external_function_param_casadi * sim_forw_vde_casadi; - external_function_param_casadi * sim_expl_ode_fun_casadi; - external_function_param_casadi * sim_expl_ode_hess; - - // IRK - external_function_param_casadi * sim_impl_dae_fun; - external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z; - external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z; - external_function_param_casadi * sim_impl_dae_hess; - - // GNSF - external_function_param_casadi * sim_gnsf_phi_fun; - external_function_param_casadi * sim_gnsf_phi_fun_jac_y; - external_function_param_casadi * sim_gnsf_phi_jac_y_uhat; - external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z; - external_function_param_casadi * sim_gnsf_get_matrices_fun; - -} sim_solver_capsule; - - -ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np); - -ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule); - - -ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule); - -#ifdef __cplusplus -} -#endif - -#endif // ACADOS_SIM_lat_H_ diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c deleted file mode 100644 index 9294d2b70..000000000 --- a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c +++ /dev/null @@ -1,264 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#define S_FUNCTION_NAME acados_solver_sfunction_lat -#define S_FUNCTION_LEVEL 2 - -#define MDL_START - -// acados -// #include "acados/utils/print.h" -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -// example specific -#include "lat_model/lat_model.h" -#include "acados_solver_lat.h" - -#include "simstruc.h" - -#define SAMPLINGTIME 0.009765625 - -static void mdlInitializeSizes (SimStruct *S) -{ - // specify the number of continuous and discrete states - ssSetNumContStates(S, 0); - ssSetNumDiscStates(S, 0);// specify the number of input ports - if ( !ssSetNumInputPorts(S, 8) ) - return; - - // specify the number of output ports - if ( !ssSetNumOutputPorts(S, 6) ) - return; - - // specify dimension information for the input ports - // lbx_0 - ssSetInputPortVectorDimension(S, 0, 4); - // ubx_0 - ssSetInputPortVectorDimension(S, 1, 4); - // parameters - ssSetInputPortVectorDimension(S, 2, (16+1) * 2); - // y_ref_0 - ssSetInputPortVectorDimension(S, 3, 5); - // y_ref - ssSetInputPortVectorDimension(S, 4, 75); - // y_ref_e - ssSetInputPortVectorDimension(S, 5, 3); - // lbx - ssSetInputPortVectorDimension(S, 6, 30); - // ubx - ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */ - ssSetOutputPortVectorDimension(S, 0, 1 ); - ssSetOutputPortVectorDimension(S, 1, 1 ); - ssSetOutputPortVectorDimension(S, 2, 1 ); - ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1 - ssSetOutputPortVectorDimension(S, 4, 1); - ssSetOutputPortVectorDimension(S, 5, 1 ); - - // specify the direct feedthrough status - // should be set to 1 for all inputs used in mdlOutputs - ssSetInputPortDirectFeedThrough(S, 0, 1); - ssSetInputPortDirectFeedThrough(S, 1, 1); - ssSetInputPortDirectFeedThrough(S, 2, 1); - ssSetInputPortDirectFeedThrough(S, 3, 1); - ssSetInputPortDirectFeedThrough(S, 4, 1); - ssSetInputPortDirectFeedThrough(S, 5, 1); - ssSetInputPortDirectFeedThrough(S, 6, 1); - ssSetInputPortDirectFeedThrough(S, 7, 1); - - // one sample time - ssSetNumSampleTimes(S, 1); -} - - -#if defined(MATLAB_MEX_FILE) - -#define MDL_SET_INPUT_PORT_DIMENSION_INFO -#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO - -static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) -{ - if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) ) - return; -} - -static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) -{ - if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) ) - return; -} - -#endif /* MATLAB_MEX_FILE */ - - -static void mdlInitializeSampleTimes(SimStruct *S) -{ - ssSetSampleTime(S, 0, SAMPLINGTIME); - ssSetOffsetTime(S, 0, 0.0); -} - - -static void mdlStart(SimStruct *S) -{ - lat_solver_capsule *capsule = lat_acados_create_capsule(); - lat_acados_create(capsule); - - ssSetUserData(S, (void*)capsule); -} - - -static void mdlOutputs(SimStruct *S, int_T tid) -{ - lat_solver_capsule *capsule = ssGetUserData(S); - ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule); - ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule); - ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule); - ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule); - - InputRealPtrsType in_sign; - - // local buffer - real_t buffer[5]; - - /* go through inputs */ - // lbx_0 - in_sign = ssGetInputPortRealSignalPtrs(S, 0); - for (int i = 0; i < 4; i++) - buffer[i] = (double)(*in_sign[i]); - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer); - // ubx_0 - in_sign = ssGetInputPortRealSignalPtrs(S, 1); - for (int i = 0; i < 4; i++) - buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer); - // parameters - stage-variant !!! - in_sign = ssGetInputPortRealSignalPtrs(S, 2); - - // update value of parameters - for (int ii = 0; ii <= 16; ii++) - { - for (int jj = 0; jj < 2; jj++) - buffer[jj] = (double)(*in_sign[ii*2+jj]); - lat_acados_update_params(capsule, ii, buffer, 2); - } - - - // y_ref_0 - in_sign = ssGetInputPortRealSignalPtrs(S, 3); - - for (int i = 0; i < 5; i++) - buffer[i] = (double)(*in_sign[i]); - - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer); - - - // y_ref - for stages 1 to N-1 - in_sign = ssGetInputPortRealSignalPtrs(S, 4); - - for (int ii = 1; ii < 16; ii++) - { - for (int jj = 0; jj < 5; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*5+jj]); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer); - } - - - // y_ref_e - in_sign = ssGetInputPortRealSignalPtrs(S, 5); - - for (int i = 0; i < 3; i++) - buffer[i] = (double)(*in_sign[i]); - - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer); - // lbx - in_sign = ssGetInputPortRealSignalPtrs(S, 6); - for (int ii = 1; ii < 16; ii++) - { - for (int jj = 0; jj < 2; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer); - } - // ubx - in_sign = ssGetInputPortRealSignalPtrs(S, 7); - for (int ii = 1; ii < 16; ii++) - { - for (int jj = 0; jj < 2; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer); - } - - /* call solver */ - int rti_phase = 0; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase); - int acados_status = lat_acados_solve(capsule); - - - /* set outputs */ - // assign pointers to output signals - real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin; - int tmp_int; - out_u0 = ssGetOutputPortRealSignal(S, 0); - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0); - - - out_status = ssGetOutputPortRealSignal(S, 1); - *out_status = (real_t) acados_status; - out_KKT_res = ssGetOutputPortRealSignal(S, 2); - *out_KKT_res = (real_t) nlp_out->inf_norm_res; - out_x1 = ssGetOutputPortRealSignal(S, 3); - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1); - out_cpu_time = ssGetOutputPortRealSignal(S, 4); - // get solution time - ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time); - out_sqp_iter = ssGetOutputPortRealSignal(S, 5); - // get sqp iter - ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int); - *out_sqp_iter = (real_t) tmp_int; - -} - -static void mdlTerminate(SimStruct *S) -{ - lat_solver_capsule *capsule = ssGetUserData(S); - - lat_acados_free(capsule); - lat_acados_free_capsule(capsule); -} - - -#ifdef MATLAB_MEX_FILE -#include "simulink.c" -#else -#include "cg_sfun.h" -#endif diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c deleted file mode 100644 index c536654fd..000000000 --- a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c +++ /dev/null @@ -1,128 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -// standard -#include -#include -// acados -#include "acados/utils/print.h" -#include "acados/utils/math.h" -#include "acados_c/sim_interface.h" -#include "acados_sim_solver_lat.h" - -#define NX LAT_NX -#define NZ LAT_NZ -#define NU LAT_NU -#define NP LAT_NP - - -int main() -{ - int status = 0; - sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule(); - status = lat_acados_sim_create(capsule); - - if (status) - { - printf("acados_create() returned status %d. Exiting.\n", status); - exit(1); - } - - sim_config *acados_sim_config = lat_acados_get_sim_config(capsule); - sim_in *acados_sim_in = lat_acados_get_sim_in(capsule); - sim_out *acados_sim_out = lat_acados_get_sim_out(capsule); - void *acados_sim_dims = lat_acados_get_sim_dims(capsule); - - // initial condition - double x_current[NX]; - x_current[0] = 0.0; - x_current[1] = 0.0; - x_current[2] = 0.0; - x_current[3] = 0.0; - - - x_current[0] = 0; - x_current[1] = 0; - x_current[2] = 0; - x_current[3] = 0; - - - - - // initial value for control input - double u0[NU]; - u0[0] = 0.0; - // set parameters - double p[NP]; - p[0] = 0; - p[1] = 0; - - lat_acados_sim_update_params(capsule, p, NP); - - - int n_sim_steps = 3; - // solve ocp in loop - for (int ii = 0; ii < n_sim_steps; ii++) - { - sim_in_set(acados_sim_config, acados_sim_dims, - acados_sim_in, "x", x_current); - status = lat_acados_sim_solve(capsule); - - if (status != ACADOS_SUCCESS) - { - printf("acados_solve() failed with status %d.\n", status); - } - - sim_out_get(acados_sim_config, acados_sim_dims, - acados_sim_out, "x", x_current); - - printf("\nx_current, %d\n", ii); - for (int jj = 0; jj < NX; jj++) - { - printf("%e\n", x_current[jj]); - } - } - - printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps); - - // free solver - status = lat_acados_sim_free(capsule); - if (status) { - printf("lat_acados_sim_free() returned status %d. \n", status); - } - - lat_acados_sim_solver_free_capsule(capsule); - - return status; -} diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m deleted file mode 100644 index f8371f3e8..000000000 --- a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m +++ /dev/null @@ -1,125 +0,0 @@ -% -% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, -% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, -% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, -% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl -% -% This file is part of acados. -% -% The 2-Clause BSD License -% -% Redistribution and use in source and binary forms, with or without -% modification, are permitted provided that the following conditions are met: -% -% 1. Redistributions of source code must retain the above copyright notice, -% this list of conditions and the following disclaimer. -% -% 2. Redistributions in binary form must reproduce the above copyright notice, -% this list of conditions and the following disclaimer in the documentation -% and/or other materials provided with the distribution. -% -% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -% POSSIBILITY OF SUCH DAMAGE.; -% - -SOURCES = { ... - 'lat_model/lat_expl_ode_fun.c', ... - 'lat_model/lat_expl_vde_forw.c',... - 'lat_cost/lat_cost_y_0_fun.c',... - 'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',... - 'lat_cost/lat_cost_y_0_hess.c',... - 'lat_cost/lat_cost_y_fun.c',... - 'lat_cost/lat_cost_y_fun_jac_ut_xt.c',... - 'lat_cost/lat_cost_y_hess.c',... - 'lat_cost/lat_cost_y_e_fun.c',... - 'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',... - 'lat_cost/lat_cost_y_e_hess.c',... - 'acados_solver_sfunction_lat.c', ... - 'acados_solver_lat.c' - }; - -INC_PATH = '/data/openpilot/third_party/acados/include/acados/include'; - -INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ... - ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ... - ['-I', fullfile(INC_PATH, 'acados')], ... - ['-I', fullfile(INC_PATH)]}; - - - -CFLAGS = 'CFLAGS=$CFLAGS'; -LDFLAGS = 'LDFLAGS=$LDFLAGS'; -COMPFLAGS = 'COMPFLAGS=$COMPFLAGS'; -COMPDEFINES = 'COMPDEFINES=$COMPDEFINES'; - - - -LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')]; - -LIBS = {'-lacados', '-lhpipm', '-lblasfeo'}; - -% acados linking libraries and flags - - -mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ... - LIB_PATH, LIBS{:}, SOURCES{:}, ... - '-output', 'acados_solver_sfunction_lat' ); - -fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ... - eval('mexext')] ); - - -%% print note on usage of s-function -fprintf('\n\nNote: Usage of Sfunction is as follows:\n') -input_note = 'Inputs are:\n'; -i_in = 1; -input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',... - ' size [4]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',... - ' size [4]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',... - ' size [34]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [5]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',... - ' size [75]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [3]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n '); -i_in = i_in + 1; - -fprintf(input_note) - -disp(' ') - -output_note = 'Outputs are:\n'; -i_out = 0; -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') KKT residual\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') CPU time\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n '); - -fprintf(output_note) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a2c8cbac7..494a18144 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -41,15 +41,15 @@ DP_ACCEL_NORMAL = 1 DP_ACCEL_SPORT = 2 # accel profile by @arne182 modified by cgw -_DP_CRUISE_MIN_V = [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0] -_DP_CRUISE_MIN_V_ECO = [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0] -_DP_CRUISE_MIN_V_SPORT = [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0] -_DP_CRUISE_MIN_BP = [0., 0.07, 6., 8., 11., 15., 20., 25., 30., 55.] +_DP_CRUISE_MIN_V = [-0.80, -0.80, -0.75, -0.75, -0.75, -0.60] +_DP_CRUISE_MIN_V_ECO = [-0.80, -0.75, -0.65, -0.65, -0.60, -0.50] +_DP_CRUISE_MIN_V_SPORT = [-1.0, -1.0, -1.0, -1.1, -1.1, -1.1] +_DP_CRUISE_MIN_BP = [0., 0.07, 10., 20., 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.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.] +_DP_CRUISE_MAX_V = [3.5, 3.4, 2.1, 1.6, 1.1, 0.91, 0.68, 0.44, 0.34, 0.13] +_DP_CRUISE_MAX_V_ECO = [3.0, 2.2, 1.4, 1.0, 0.75, 0.6, 0.48, 0.36, 0.28, 0.09] +_DP_CRUISE_MAX_V_SPORT = [3.5, 3.5, 3.4, 3.0, 2.1, 1.61, 1.1, 0.63, 0.50, 0.33] +_DP_CRUISE_MAX_BP = [0., 3, 6., 8., 11., 15., 20., 25., 30., 55.] # d-e2e, from modeldata.h TRAJECTORY_SIZE = 33 @@ -99,7 +99,20 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0): - # dp - conditional e2e + # dp + self.dp_accel_profile_ctrl = False + self.dp_accel_profile = DP_ACCEL_ECO + self.dp_following_profile_ctrl = False + self.dp_following_profile = 0 + self.cruise_source = 'cruise' + self.vision_turn_controller = VisionTurnController(CP) + self.speed_limit_controller = SpeedLimitController() + self.events = Events() + self.turn_speed_controller = TurnSpeedController() + self.dp_e2e_adapt_ap = False + self.dp_e2e_adapt_fp = False + + # conditional e2e self.dp_e2e_has_lead = False self.dp_e2e_lead_last = False self.dp_e2e_lead_count = 0 @@ -113,36 +126,19 @@ class LongitudinalPlanner: self.CP = CP - self.params = Params() - self.param_read_counter = 0 self.mpc = LongitudinalMpc() - self.read_param() self.fcw = False 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) self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 - # dp - self.dp_accel_profile_ctrl = False - self.dp_accel_profile = DP_ACCEL_ECO - self.dp_following_profile_ctrl = False - self.dp_following_profile = 0 - self.cruise_source = 'cruise' - self.vision_turn_controller = VisionTurnController(CP) - self.speed_limit_controller = SpeedLimitController() - self.events = Events() - self.turn_speed_controller = TurnSpeedController() - self.dp_e2e_adapt_ap = False - self.dp_e2e_adapt_fp = False - def read_param(self): e2e = self.params.get_bool('ExperimentalMode') and self.CP.openpilotLongitudinalControl self.mpc.mode = 'blended' if e2e else 'acc' @@ -164,8 +160,6 @@ class LongitudinalPlanner: self.dp_e2e_swap_count += 1 if self.dp_e2e_swap_count >= _DP_E2E_SWAP_COUNT: - # if self.mpc.mode != mode: - # reset_state = True self.mpc.mode = mode reset_state = True @@ -203,7 +197,7 @@ class LongitudinalPlanner: if self.dp_e2e_sng_count > _DP_E2E_SNG_COUNT: if self.dp_e2e_sng_count > _DP_E2E_SNG_ACC_COUNT: self.dp_e2e_sng = False - return self._set_dp_e2e_mode('acc') + return self._set_dp_e2e_mode('acc', True) return self._set_dp_e2e_mode('blended') # when we see a lead @@ -214,7 +208,7 @@ class LongitudinalPlanner: else: self.dp_e2e_tf_count = 0 if self.dp_e2e_tf_count > _DP_E2E_TF_COUNT: - return self._set_dp_e2e_mode('blended') + return self._set_dp_e2e_mode('blended', True) # stop sign detection md = sm['modelV2'] @@ -227,17 +221,17 @@ class LongitudinalPlanner: self.dp_e2e_stop_count = 0 if self.dp_e2e_stop_count >= _DP_E2E_STOP_COUNT: - return self._set_dp_e2e_mode('blended') + return self._set_dp_e2e_mode('blended', True) return self._set_dp_e2e_mode('acc') @staticmethod - def parse_model(model_msg): #, model_error): + def parse_model(model_msg): 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) #- model_error * T_IDXS_MPC - v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) #- model_error + x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) + v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x) j = np.zeros(len(T_IDXS_MPC)) else: @@ -253,27 +247,20 @@ class LongitudinalPlanner: 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 - x_vel = [0, 5.56, 11.11, 13.89, 19.4, 22.2, 25.0, 41.67] - y_dist = [1.2, 1.3, 1.32, 1.32, 1.32, 1.32, 1.32, 1.35] + x_vel = [0, 3., 13.89, 25.0, 41.67] + y_dist = [1.24, 1.34, 1.36, 1.26, 1.32] desired_tf = np.interp(v_ego, x_vel, y_dist) elif self.dp_following_profile == 1: - # in kph ~= 0 20 40 50 70 90 150 - #x_vel = [0, 5.56, 11.11, 13.89, 19.4, 25.0, 41.67] - #y_dist = [1.3, 1.4, 1.45, 1.5, 1.5, 1.6, 1.8] - # in kph ~= 0 20 40 50 70 90 150 - x_vel = [0, 5.56, 11.11, 13.89, 19.4, 25.0, 41.67] - y_dist = [1.2, 1.37, 1.45, 1.5, 1.5, 1.6, 1.8] + x_vel = [0, 5.556, 13.89, 41.67] + y_dist = [1.3, 1.460, 1.5000, 1.68] desired_tf = np.interp(v_ego, x_vel, y_dist) elif self.dp_following_profile == 2: - # in kph ~= 0 20 40 50 90 150 - x_vel = [0, 5.56, 11.11, 13.89, 25.0, 41.67] - y_dist = [1.2, 1.47, 1.75, 1.95, 2.2, 2.4] + x_vel = [0, 5.556, 19.7, 41.67] + y_dist = [1.4, 1.54, 2.0, 2.2] desired_tf = np.interp(v_ego, x_vel, y_dist) return desired_tf - def update(self, sm, read=True): + def update(self, sm): # dp self.dp_accel_profile_ctrl = sm['dragonConf'].dpAccelProfileCtrl self.dp_accel_profile = sm['dragonConf'].dpAccelProfile @@ -286,9 +273,7 @@ class LongitudinalPlanner: self.dp_e2e_adapt_fp = sm['dragonConf'].dpE2EConditionalAdaptFp dp_reset_state = self.conditional_e2e(sm) else: - if self.param_read_counter % 50 == 0 and read: - self.read_param() - self.param_read_counter += 1 + self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo v_cruise_kph = sm['controlsState'].vCruise @@ -345,7 +330,7 @@ class LongitudinalPlanner: # 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']) #, self.v_model_error) + x, v, a, j = self.parse_model(sm['modelV2']) self.dp_e2e_tf = self.get_df(v_ego) self.mpc.update(sm['carState'], sm['radarState'], v_cruise_sol, x, v, a, j, prev_accel_constraint, self.dp_e2e_tf) @@ -357,6 +342,8 @@ class LongitudinalPlanner: self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill if self.fcw: cloudlog.info("FCW triggered") + if sm['dragonConf'].dpE2EConditional: + self._set_dp_e2e_mode('blended', True) # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json deleted file mode 100644 index 1cbe32253..000000000 --- a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json +++ /dev/null @@ -1,450 +0,0 @@ -{ - "acados_include_path": "/data/openpilot/third_party/acados/include/acados/include", - "acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib", - "code_export_directory": "/data/openpilot/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code", - "constraints": { - "C": [], - "C_e": [], - "D": [], - "constr_type": "BGH", - "constr_type_e": "BGH", - "idxbu": [], - "idxbx": [ - 2, - 3 - ], - "idxbx_0": [ - 0, - 1, - 2, - 3 - ], - "idxbx_e": [], - "idxbxe_0": [ - 0, - 1, - 2, - 3 - ], - "idxsbu": [], - "idxsbx": [], - "idxsbx_e": [], - "idxsg": [], - "idxsg_e": [], - "idxsh": [], - "idxsh_e": [], - "idxsphi": [], - "idxsphi_e": [], - "lbu": [], - "lbx": [ - -1.5707963267948966, - -0.8726646259971648 - ], - "lbx_0": [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - "lbx_e": [], - "lg": [], - "lg_e": [], - "lh": [], - "lh_e": [], - "lphi": [], - "lphi_e": [], - "lsbu": [], - "lsbx": [], - "lsbx_e": [], - "lsg": [], - "lsg_e": [], - "lsh": [], - "lsh_e": [], - "lsphi": [], - "lsphi_e": [], - "ubu": [], - "ubx": [ - 1.5707963267948966, - 0.8726646259971648 - ], - "ubx_0": [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - "ubx_e": [], - "ug": [], - "ug_e": [], - "uh": [], - "uh_e": [], - "uphi": [], - "uphi_e": [], - "usbu": [], - "usbx": [], - "usbx_e": [], - "usg": [], - "usg_e": [], - "ush": [], - "ush_e": [], - "usphi": [], - "usphi_e": [] - }, - "cost": { - "Vu": [ - [ - 0.0 - ], - [ - 0.0 - ], - [ - 0.0 - ] - ], - "Vu_0": [ - [ - 0.0 - ], - [ - 0.0 - ], - [ - 0.0 - ] - ], - "Vx": [ - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_0": [ - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_e": [ - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vz": [], - "Vz_0": [], - "W": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ] - ], - "W_0": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ] - ], - "W_e": [ - [ - 0.0, - 0.0 - ], - [ - 0.0, - 0.0 - ] - ], - "Zl": [], - "Zl_e": [], - "Zu": [], - "Zu_e": [], - "cost_ext_fun_type": "casadi", - "cost_ext_fun_type_0": "casadi", - "cost_ext_fun_type_e": "casadi", - "cost_type": "NONLINEAR_LS", - "cost_type_0": "NONLINEAR_LS", - "cost_type_e": "NONLINEAR_LS", - "yref": [ - 0.0, - 0.0, - 0.0 - ], - "yref_0": [ - 0.0, - 0.0, - 0.0 - ], - "yref_e": [ - 0.0, - 0.0 - ], - "zl": [], - "zl_e": [], - "zu": [], - "zu_e": [] - }, - "cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include", - "dims": { - "N": 16, - "nbu": 0, - "nbx": 2, - "nbx_0": 4, - "nbx_e": 0, - "nbxe_0": 4, - "ng": 0, - "ng_e": 0, - "nh": 0, - "nh_e": 0, - "np": 2, - "nphi": 0, - "nphi_e": 0, - "nr": 0, - "nr_e": 0, - "ns": 0, - "ns_e": 0, - "nsbu": 0, - "nsbx": 0, - "nsbx_e": 0, - "nsg": 0, - "nsg_e": 0, - "nsh": 0, - "nsh_e": 0, - "nsphi": 0, - "nsphi_e": 0, - "nu": 1, - "nx": 4, - "ny": 3, - "ny_0": 3, - "ny_e": 2, - "nz": 0 - }, - "model": { - "dyn_disc_fun": null, - "dyn_disc_fun_jac": null, - "dyn_disc_fun_jac_hess": null, - "dyn_ext_fun_type": "casadi", - "dyn_source_discrete": null, - "gnsf": { - "nontrivial_f_LO": 1, - "purely_linear": 0 - }, - "name": "lat" - }, - "parameter_values": [ - 0.0, - 0.0 - ], - "problem_class": "OCP", - "simulink_opts": { - "inputs": { - "cost_W": 0, - "cost_W_0": 0, - "cost_W_e": 0, - "lbu": 1, - "lbx": 1, - "lbx_0": 1, - "lbx_e": 1, - "lg": 1, - "lh": 1, - "parameter_traj": 1, - "reset_solver": 0, - "u_init": 0, - "ubu": 1, - "ubx": 1, - "ubx_0": 1, - "ubx_e": 1, - "ug": 1, - "uh": 1, - "x_init": 0, - "y_ref": 1, - "y_ref_0": 1, - "y_ref_e": 1 - }, - "outputs": { - "CPU_time": 1, - "CPU_time_lin": 0, - "CPU_time_qp": 0, - "CPU_time_sim": 0, - "KKT_residual": 1, - "solver_status": 1, - "sqp_iter": 1, - "u0": 1, - "utraj": 0, - "x1": 1, - "xtraj": 0 - }, - "samplingtime": "t0" - }, - "solver_options": { - "Tsim": 0.009765625, - "alpha_min": 0.05, - "alpha_reduction": 0.7, - "collocation_type": "GAUSS_LEGENDRE", - "eps_sufficient_descent": 0.0001, - "exact_hess_constr": 1, - "exact_hess_cost": 1, - "exact_hess_dyn": 1, - "ext_cost_num_hess": 0, - "full_step_dual": 0, - "globalization": "FIXED_STEP", - "globalization_use_SOC": 0, - "hessian_approx": "GAUSS_NEWTON", - "hpipm_mode": "BALANCE", - "initialize_t_slacks": 0, - "integrator_type": "ERK", - "levenberg_marquardt": 0.0, - "line_search_use_sufficient_descent": 0, - "model_external_shared_lib_dir": null, - "model_external_shared_lib_name": null, - "nlp_solver_max_iter": 100, - "nlp_solver_step_length": 1.0, - "nlp_solver_tol_comp": 1e-06, - "nlp_solver_tol_eq": 1e-06, - "nlp_solver_tol_ineq": 1e-06, - "nlp_solver_tol_stat": 1e-06, - "nlp_solver_type": "SQP_RTI", - "print_level": 0, - "qp_solver": "PARTIAL_CONDENSING_HPIPM", - "qp_solver_cond_N": 1, - "qp_solver_iter_max": 1, - "qp_solver_tol_comp": null, - "qp_solver_tol_eq": null, - "qp_solver_tol_ineq": null, - "qp_solver_tol_stat": null, - "qp_solver_warm_start": 0, - "regularize_method": null, - "sim_method_jac_reuse": [ - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0 - ], - "sim_method_newton_iter": 3, - "sim_method_num_stages": [ - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4 - ], - "sim_method_num_steps": [ - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1 - ], - "tf": 2.5, - "time_steps": [ - 0.009765625, - 0.029296875, - 0.048828125, - 0.068359375, - 0.087890625, - 0.107421875, - 0.126953125, - 0.146484375, - 0.166015625, - 0.185546875, - 0.205078125, - 0.224609375, - 0.244140625, - 0.263671875, - 0.283203125, - 0.302734375 - ] - } -} \ No newline at end of file diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c deleted file mode 100644 index d0c541d3b..000000000 --- a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c +++ /dev/null @@ -1,278 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ -// standard -#include -#include - -// acados -#include "acados_c/external_function_interface.h" -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -#include "acados/sim/sim_common.h" -#include "acados/utils/external_function_generic.h" -#include "acados/utils/print.h" - - -// example specific -#include "lat_model/lat_model.h" -#include "acados_sim_solver_lat.h" - - -// ** solver data ** - -sim_solver_capsule * lat_acados_sim_solver_create_capsule() -{ - void* capsule_mem = malloc(sizeof(sim_solver_capsule)); - sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem; - - return capsule; -} - - -int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule) -{ - free(capsule); - return 0; -} - - -int lat_acados_sim_create(sim_solver_capsule * capsule) -{ - // initialize - const int nx = LAT_NX; - const int nu = LAT_NU; - const int nz = LAT_NZ; - const int np = LAT_NP; - bool tmp_bool; - - - double Tsim = 0.009765625; - - - // explicit ode - capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - - capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw; - capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in; - capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out; - capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in; - capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out; - capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work; - external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np); - - capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun; - capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in; - capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out; - capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in; - capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out; - capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work; - external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np); - - - - // sim plan & config - sim_solver_plan_t plan; - plan.sim_solver = ERK; - - // create correct config based on plan - sim_config * lat_sim_config = sim_config_create(plan); - capsule->acados_sim_config = lat_sim_config; - - // sim dims - void *lat_sim_dims = sim_dims_create(lat_sim_config); - capsule->acados_sim_dims = lat_sim_dims; - sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx); - sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu); - sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz); - - - // sim opts - sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims); - capsule->acados_sim_opts = lat_sim_opts; - int tmp_int = 3; - sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int); - sim_collocation_type collocation_type = GAUSS_LEGENDRE; - sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type); - - - tmp_int = 4; - sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int); - tmp_int = 1; - sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int); - tmp_bool = 0; - sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool); - - - // sim in / out - sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims); - capsule->acados_sim_in = lat_sim_in; - sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims); - capsule->acados_sim_out = lat_sim_out; - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "T", &Tsim); - - // model functions - lat_sim_config->model_set(lat_sim_in->model, - "expl_vde_for", capsule->sim_forw_vde_casadi); - lat_sim_config->model_set(lat_sim_in->model, - "expl_ode_fun", capsule->sim_expl_ode_fun_casadi); - - // sim solver - sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config, - lat_sim_dims, lat_sim_opts); - capsule->acados_sim_solver = lat_sim_solver; - - - /* initialize parameter values */ - double* p = calloc(np, sizeof(double)); - - - lat_acados_sim_update_params(capsule, p, np); - free(p); - - - /* initialize input */ - // x - double x0[4]; - for (int ii = 0; ii < 4; ii++) - x0[ii] = 0.0; - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "x", x0); - - - // u - double u0[1]; - for (int ii = 0; ii < 1; ii++) - u0[ii] = 0.0; - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "u", u0); - - // S_forw - double S_forw[20]; - for (int ii = 0; ii < 20; ii++) - S_forw[ii] = 0.0; - for (int ii = 0; ii < 4; ii++) - S_forw[ii + ii * 4 ] = 1.0; - - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "S_forw", S_forw); - - int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out); - - return status; -} - - -int lat_acados_sim_solve(sim_solver_capsule *capsule) -{ - // integrate dynamics using acados sim_solver - int status = sim_solve(capsule->acados_sim_solver, - capsule->acados_sim_in, capsule->acados_sim_out); - if (status != 0) - printf("error in lat_acados_sim_solve()! Exiting.\n"); - - return status; -} - - -int lat_acados_sim_free(sim_solver_capsule *capsule) -{ - // free memory - sim_solver_destroy(capsule->acados_sim_solver); - sim_in_destroy(capsule->acados_sim_in); - sim_out_destroy(capsule->acados_sim_out); - sim_opts_destroy(capsule->acados_sim_opts); - sim_dims_destroy(capsule->acados_sim_dims); - sim_config_destroy(capsule->acados_sim_config); - - // free external function - external_function_param_casadi_free(capsule->sim_forw_vde_casadi); - external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi); - - return 0; -} - - -int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np) -{ - int status = 0; - int casadi_np = LAT_NP; - - if (casadi_np != np) { - printf("lat_acados_sim_update_params: trying to set %i parameters for external functions." - " External function has %i parameters. Exiting.\n", np, casadi_np); - exit(1); - } - capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p); - capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p); - - return status; -} - -/* getters pointers to C objects*/ -sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_config; -}; - -sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_in; -}; - -sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_out; -}; - -void * lat_acados_get_sim_dims(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_dims; -}; - -sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_opts; -}; - -sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_solver; -}; - diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h deleted file mode 100644 index 86b9c84c9..000000000 --- a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#ifndef ACADOS_SIM_lat_H_ -#define ACADOS_SIM_lat_H_ - -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -#define LAT_NX 4 -#define LAT_NZ 0 -#define LAT_NU 1 -#define LAT_NP 2 - -#ifdef __cplusplus -extern "C" { -#endif - - -// ** capsule for solver data ** -typedef struct sim_solver_capsule -{ - // acados objects - sim_in *acados_sim_in; - sim_out *acados_sim_out; - sim_solver *acados_sim_solver; - sim_opts *acados_sim_opts; - sim_config *acados_sim_config; - void *acados_sim_dims; - - /* external functions */ - // ERK - external_function_param_casadi * sim_forw_vde_casadi; - external_function_param_casadi * sim_expl_ode_fun_casadi; - external_function_param_casadi * sim_expl_ode_hess; - - // IRK - external_function_param_casadi * sim_impl_dae_fun; - external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z; - external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z; - external_function_param_casadi * sim_impl_dae_hess; - - // GNSF - external_function_param_casadi * sim_gnsf_phi_fun; - external_function_param_casadi * sim_gnsf_phi_fun_jac_y; - external_function_param_casadi * sim_gnsf_phi_jac_y_uhat; - external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z; - external_function_param_casadi * sim_gnsf_get_matrices_fun; - -} sim_solver_capsule; - - -ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np); - -ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule); - - -ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule); - -#ifdef __cplusplus -} -#endif - -#endif // ACADOS_SIM_lat_H_ diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c deleted file mode 100644 index 827b6c7f0..000000000 --- a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c +++ /dev/null @@ -1,264 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#define S_FUNCTION_NAME acados_solver_sfunction_lat -#define S_FUNCTION_LEVEL 2 - -#define MDL_START - -// acados -// #include "acados/utils/print.h" -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -// example specific -#include "lat_model/lat_model.h" -#include "acados_solver_lat.h" - -#include "simstruc.h" - -#define SAMPLINGTIME 0.009765625 - -static void mdlInitializeSizes (SimStruct *S) -{ - // specify the number of continuous and discrete states - ssSetNumContStates(S, 0); - ssSetNumDiscStates(S, 0);// specify the number of input ports - if ( !ssSetNumInputPorts(S, 8) ) - return; - - // specify the number of output ports - if ( !ssSetNumOutputPorts(S, 6) ) - return; - - // specify dimension information for the input ports - // lbx_0 - ssSetInputPortVectorDimension(S, 0, 4); - // ubx_0 - ssSetInputPortVectorDimension(S, 1, 4); - // parameters - ssSetInputPortVectorDimension(S, 2, (16+1) * 2); - // y_ref_0 - ssSetInputPortVectorDimension(S, 3, 3); - // y_ref - ssSetInputPortVectorDimension(S, 4, 45); - // y_ref_e - ssSetInputPortVectorDimension(S, 5, 2); - // lbx - ssSetInputPortVectorDimension(S, 6, 30); - // ubx - ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */ - ssSetOutputPortVectorDimension(S, 0, 1 ); - ssSetOutputPortVectorDimension(S, 1, 1 ); - ssSetOutputPortVectorDimension(S, 2, 1 ); - ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1 - ssSetOutputPortVectorDimension(S, 4, 1); - ssSetOutputPortVectorDimension(S, 5, 1 ); - - // specify the direct feedthrough status - // should be set to 1 for all inputs used in mdlOutputs - ssSetInputPortDirectFeedThrough(S, 0, 1); - ssSetInputPortDirectFeedThrough(S, 1, 1); - ssSetInputPortDirectFeedThrough(S, 2, 1); - ssSetInputPortDirectFeedThrough(S, 3, 1); - ssSetInputPortDirectFeedThrough(S, 4, 1); - ssSetInputPortDirectFeedThrough(S, 5, 1); - ssSetInputPortDirectFeedThrough(S, 6, 1); - ssSetInputPortDirectFeedThrough(S, 7, 1); - - // one sample time - ssSetNumSampleTimes(S, 1); -} - - -#if defined(MATLAB_MEX_FILE) - -#define MDL_SET_INPUT_PORT_DIMENSION_INFO -#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO - -static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) -{ - if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) ) - return; -} - -static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) -{ - if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) ) - return; -} - -#endif /* MATLAB_MEX_FILE */ - - -static void mdlInitializeSampleTimes(SimStruct *S) -{ - ssSetSampleTime(S, 0, SAMPLINGTIME); - ssSetOffsetTime(S, 0, 0.0); -} - - -static void mdlStart(SimStruct *S) -{ - lat_solver_capsule *capsule = lat_acados_create_capsule(); - lat_acados_create(capsule); - - ssSetUserData(S, (void*)capsule); -} - - -static void mdlOutputs(SimStruct *S, int_T tid) -{ - lat_solver_capsule *capsule = ssGetUserData(S); - ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule); - ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule); - ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule); - ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule); - - InputRealPtrsType in_sign; - - // local buffer - real_t buffer[4]; - - /* go through inputs */ - // lbx_0 - in_sign = ssGetInputPortRealSignalPtrs(S, 0); - for (int i = 0; i < 4; i++) - buffer[i] = (double)(*in_sign[i]); - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer); - // ubx_0 - in_sign = ssGetInputPortRealSignalPtrs(S, 1); - for (int i = 0; i < 4; i++) - buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer); - // parameters - stage-variant !!! - in_sign = ssGetInputPortRealSignalPtrs(S, 2); - - // update value of parameters - for (int ii = 0; ii <= 16; ii++) - { - for (int jj = 0; jj < 2; jj++) - buffer[jj] = (double)(*in_sign[ii*2+jj]); - lat_acados_update_params(capsule, ii, buffer, 2); - } - - - // y_ref_0 - in_sign = ssGetInputPortRealSignalPtrs(S, 3); - - for (int i = 0; i < 3; i++) - buffer[i] = (double)(*in_sign[i]); - - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer); - - - // y_ref - for stages 1 to N-1 - in_sign = ssGetInputPortRealSignalPtrs(S, 4); - - for (int ii = 1; ii < 16; ii++) - { - for (int jj = 0; jj < 3; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer); - } - - - // y_ref_e - in_sign = ssGetInputPortRealSignalPtrs(S, 5); - - for (int i = 0; i < 2; i++) - buffer[i] = (double)(*in_sign[i]); - - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer); - // lbx - in_sign = ssGetInputPortRealSignalPtrs(S, 6); - for (int ii = 1; ii < 16; ii++) - { - for (int jj = 0; jj < 2; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer); - } - // ubx - in_sign = ssGetInputPortRealSignalPtrs(S, 7); - for (int ii = 1; ii < 16; ii++) - { - for (int jj = 0; jj < 2; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer); - } - - /* call solver */ - int rti_phase = 0; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase); - int acados_status = lat_acados_solve(capsule); - - - /* set outputs */ - // assign pointers to output signals - real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin; - int tmp_int; - out_u0 = ssGetOutputPortRealSignal(S, 0); - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0); - - - out_status = ssGetOutputPortRealSignal(S, 1); - *out_status = (real_t) acados_status; - out_KKT_res = ssGetOutputPortRealSignal(S, 2); - *out_KKT_res = (real_t) nlp_out->inf_norm_res; - out_x1 = ssGetOutputPortRealSignal(S, 3); - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1); - out_cpu_time = ssGetOutputPortRealSignal(S, 4); - // get solution time - ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time); - out_sqp_iter = ssGetOutputPortRealSignal(S, 5); - // get sqp iter - ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int); - *out_sqp_iter = (real_t) tmp_int; - -} - -static void mdlTerminate(SimStruct *S) -{ - lat_solver_capsule *capsule = ssGetUserData(S); - - lat_acados_free(capsule); - lat_acados_free_capsule(capsule); -} - - -#ifdef MATLAB_MEX_FILE -#include "simulink.c" -#else -#include "cg_sfun.h" -#endif diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c deleted file mode 100644 index c536654fd..000000000 --- a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c +++ /dev/null @@ -1,128 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -// standard -#include -#include -// acados -#include "acados/utils/print.h" -#include "acados/utils/math.h" -#include "acados_c/sim_interface.h" -#include "acados_sim_solver_lat.h" - -#define NX LAT_NX -#define NZ LAT_NZ -#define NU LAT_NU -#define NP LAT_NP - - -int main() -{ - int status = 0; - sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule(); - status = lat_acados_sim_create(capsule); - - if (status) - { - printf("acados_create() returned status %d. Exiting.\n", status); - exit(1); - } - - sim_config *acados_sim_config = lat_acados_get_sim_config(capsule); - sim_in *acados_sim_in = lat_acados_get_sim_in(capsule); - sim_out *acados_sim_out = lat_acados_get_sim_out(capsule); - void *acados_sim_dims = lat_acados_get_sim_dims(capsule); - - // initial condition - double x_current[NX]; - x_current[0] = 0.0; - x_current[1] = 0.0; - x_current[2] = 0.0; - x_current[3] = 0.0; - - - x_current[0] = 0; - x_current[1] = 0; - x_current[2] = 0; - x_current[3] = 0; - - - - - // initial value for control input - double u0[NU]; - u0[0] = 0.0; - // set parameters - double p[NP]; - p[0] = 0; - p[1] = 0; - - lat_acados_sim_update_params(capsule, p, NP); - - - int n_sim_steps = 3; - // solve ocp in loop - for (int ii = 0; ii < n_sim_steps; ii++) - { - sim_in_set(acados_sim_config, acados_sim_dims, - acados_sim_in, "x", x_current); - status = lat_acados_sim_solve(capsule); - - if (status != ACADOS_SUCCESS) - { - printf("acados_solve() failed with status %d.\n", status); - } - - sim_out_get(acados_sim_config, acados_sim_dims, - acados_sim_out, "x", x_current); - - printf("\nx_current, %d\n", ii); - for (int jj = 0; jj < NX; jj++) - { - printf("%e\n", x_current[jj]); - } - } - - printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps); - - // free solver - status = lat_acados_sim_free(capsule); - if (status) { - printf("lat_acados_sim_free() returned status %d. \n", status); - } - - lat_acados_sim_solver_free_capsule(capsule); - - return status; -} diff --git a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m b/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m deleted file mode 100644 index 2c6679ecb..000000000 --- a/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m +++ /dev/null @@ -1,125 +0,0 @@ -% -% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, -% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, -% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, -% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl -% -% This file is part of acados. -% -% The 2-Clause BSD License -% -% Redistribution and use in source and binary forms, with or without -% modification, are permitted provided that the following conditions are met: -% -% 1. Redistributions of source code must retain the above copyright notice, -% this list of conditions and the following disclaimer. -% -% 2. Redistributions in binary form must reproduce the above copyright notice, -% this list of conditions and the following disclaimer in the documentation -% and/or other materials provided with the distribution. -% -% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -% POSSIBILITY OF SUCH DAMAGE.; -% - -SOURCES = { ... - 'lat_model/lat_expl_ode_fun.c', ... - 'lat_model/lat_expl_vde_forw.c',... - 'lat_cost/lat_cost_y_0_fun.c',... - 'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',... - 'lat_cost/lat_cost_y_0_hess.c',... - 'lat_cost/lat_cost_y_fun.c',... - 'lat_cost/lat_cost_y_fun_jac_ut_xt.c',... - 'lat_cost/lat_cost_y_hess.c',... - 'lat_cost/lat_cost_y_e_fun.c',... - 'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',... - 'lat_cost/lat_cost_y_e_hess.c',... - 'acados_solver_sfunction_lat.c', ... - 'acados_solver_lat.c' - }; - -INC_PATH = '/data/openpilot/third_party/acados/include/acados/include'; - -INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ... - ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ... - ['-I', fullfile(INC_PATH, 'acados')], ... - ['-I', fullfile(INC_PATH)]}; - - - -CFLAGS = 'CFLAGS=$CFLAGS'; -LDFLAGS = 'LDFLAGS=$LDFLAGS'; -COMPFLAGS = 'COMPFLAGS=$COMPFLAGS'; -COMPDEFINES = 'COMPDEFINES=$COMPDEFINES'; - - - -LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')]; - -LIBS = {'-lacados', '-lhpipm', '-lblasfeo'}; - -% acados linking libraries and flags - - -mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ... - LIB_PATH, LIBS{:}, SOURCES{:}, ... - '-output', 'acados_solver_sfunction_lat' ); - -fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ... - eval('mexext')] ); - - -%% print note on usage of s-function -fprintf('\n\nNote: Usage of Sfunction is as follows:\n') -input_note = 'Inputs are:\n'; -i_in = 1; -input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',... - ' size [4]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',... - ' size [4]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',... - ' size [34]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',... - ' size [45]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n '); -i_in = i_in + 1; - -fprintf(input_note) - -disp(' ') - -output_note = 'Outputs are:\n'; -i_out = 0; -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') KKT residual\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') CPU time\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n '); - -fprintf(output_note) diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json deleted file mode 100644 index f7b5bb7f8..000000000 --- a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json +++ /dev/null @@ -1,450 +0,0 @@ -{ - "acados_include_path": "/data/openpilot/third_party/acados/include/acados/include", - "acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib", - "code_export_directory": "/data/openpilot/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code", - "constraints": { - "C": [], - "C_e": [], - "D": [], - "constr_type": "BGH", - "constr_type_e": "BGH", - "idxbu": [], - "idxbx": [ - 2, - 3 - ], - "idxbx_0": [ - 0, - 1, - 2, - 3 - ], - "idxbx_e": [], - "idxbxe_0": [ - 0, - 1, - 2, - 3 - ], - "idxsbu": [], - "idxsbx": [], - "idxsbx_e": [], - "idxsg": [], - "idxsg_e": [], - "idxsh": [], - "idxsh_e": [], - "idxsphi": [], - "idxsphi_e": [], - "lbu": [], - "lbx": [ - -1.5707963267948966, - -0.8726646259971648 - ], - "lbx_0": [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - "lbx_e": [], - "lg": [], - "lg_e": [], - "lh": [], - "lh_e": [], - "lphi": [], - "lphi_e": [], - "lsbu": [], - "lsbx": [], - "lsbx_e": [], - "lsg": [], - "lsg_e": [], - "lsh": [], - "lsh_e": [], - "lsphi": [], - "lsphi_e": [], - "ubu": [], - "ubx": [ - 1.5707963267948966, - 0.8726646259971648 - ], - "ubx_0": [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - "ubx_e": [], - "ug": [], - "ug_e": [], - "uh": [], - "uh_e": [], - "uphi": [], - "uphi_e": [], - "usbu": [], - "usbx": [], - "usbx_e": [], - "usg": [], - "usg_e": [], - "ush": [], - "ush_e": [], - "usphi": [], - "usphi_e": [] - }, - "cost": { - "Vu": [ - [ - 0.0 - ], - [ - 0.0 - ], - [ - 0.0 - ] - ], - "Vu_0": [ - [ - 0.0 - ], - [ - 0.0 - ], - [ - 0.0 - ] - ], - "Vx": [ - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_0": [ - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vx_e": [ - [ - 0.0, - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0, - 0.0 - ] - ], - "Vz": [], - "Vz_0": [], - "W": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ] - ], - "W_0": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ] - ], - "W_e": [ - [ - 0.0, - 0.0 - ], - [ - 0.0, - 0.0 - ] - ], - "Zl": [], - "Zl_e": [], - "Zu": [], - "Zu_e": [], - "cost_ext_fun_type": "casadi", - "cost_ext_fun_type_0": "casadi", - "cost_ext_fun_type_e": "casadi", - "cost_type": "NONLINEAR_LS", - "cost_type_0": "NONLINEAR_LS", - "cost_type_e": "NONLINEAR_LS", - "yref": [ - 0.0, - 0.0, - 0.0 - ], - "yref_0": [ - 0.0, - 0.0, - 0.0 - ], - "yref_e": [ - 0.0, - 0.0 - ], - "zl": [], - "zl_e": [], - "zu": [], - "zu_e": [] - }, - "cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include", - "dims": { - "N": 16, - "nbu": 0, - "nbx": 2, - "nbx_0": 4, - "nbx_e": 0, - "nbxe_0": 4, - "ng": 0, - "ng_e": 0, - "nh": 0, - "nh_e": 0, - "np": 2, - "nphi": 0, - "nphi_e": 0, - "nr": 0, - "nr_e": 0, - "ns": 0, - "ns_e": 0, - "nsbu": 0, - "nsbx": 0, - "nsbx_e": 0, - "nsg": 0, - "nsg_e": 0, - "nsh": 0, - "nsh_e": 0, - "nsphi": 0, - "nsphi_e": 0, - "nu": 1, - "nx": 4, - "ny": 3, - "ny_0": 3, - "ny_e": 2, - "nz": 0 - }, - "model": { - "dyn_disc_fun": null, - "dyn_disc_fun_jac": null, - "dyn_disc_fun_jac_hess": null, - "dyn_ext_fun_type": "casadi", - "dyn_source_discrete": null, - "gnsf": { - "nontrivial_f_LO": 1, - "purely_linear": 0 - }, - "name": "lat" - }, - "parameter_values": [ - 0.0, - 0.0 - ], - "problem_class": "OCP", - "simulink_opts": { - "inputs": { - "cost_W": 0, - "cost_W_0": 0, - "cost_W_e": 0, - "lbu": 1, - "lbx": 1, - "lbx_0": 1, - "lbx_e": 1, - "lg": 1, - "lh": 1, - "parameter_traj": 1, - "reset_solver": 0, - "u_init": 0, - "ubu": 1, - "ubx": 1, - "ubx_0": 1, - "ubx_e": 1, - "ug": 1, - "uh": 1, - "x_init": 0, - "y_ref": 1, - "y_ref_0": 1, - "y_ref_e": 1 - }, - "outputs": { - "CPU_time": 1, - "CPU_time_lin": 0, - "CPU_time_qp": 0, - "CPU_time_sim": 0, - "KKT_residual": 1, - "solver_status": 1, - "sqp_iter": 1, - "u0": 1, - "utraj": 0, - "x1": 1, - "xtraj": 0 - }, - "samplingtime": "t0" - }, - "solver_options": { - "Tsim": 0.009765625, - "alpha_min": 0.05, - "alpha_reduction": 0.7, - "collocation_type": "GAUSS_LEGENDRE", - "eps_sufficient_descent": 0.0001, - "exact_hess_constr": 1, - "exact_hess_cost": 1, - "exact_hess_dyn": 1, - "ext_cost_num_hess": 0, - "full_step_dual": 0, - "globalization": "FIXED_STEP", - "globalization_use_SOC": 0, - "hessian_approx": "GAUSS_NEWTON", - "hpipm_mode": "BALANCE", - "initialize_t_slacks": 0, - "integrator_type": "ERK", - "levenberg_marquardt": 0.0, - "line_search_use_sufficient_descent": 0, - "model_external_shared_lib_dir": null, - "model_external_shared_lib_name": null, - "nlp_solver_max_iter": 100, - "nlp_solver_step_length": 1.0, - "nlp_solver_tol_comp": 1e-06, - "nlp_solver_tol_eq": 1e-06, - "nlp_solver_tol_ineq": 1e-06, - "nlp_solver_tol_stat": 1e-06, - "nlp_solver_type": "SQP_RTI", - "print_level": 0, - "qp_solver": "PARTIAL_CONDENSING_HPIPM", - "qp_solver_cond_N": 1, - "qp_solver_iter_max": 1, - "qp_solver_tol_comp": null, - "qp_solver_tol_eq": null, - "qp_solver_tol_ineq": null, - "qp_solver_tol_stat": null, - "qp_solver_warm_start": 0, - "regularize_method": null, - "sim_method_jac_reuse": [ - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0 - ], - "sim_method_newton_iter": 3, - "sim_method_num_stages": [ - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4, - 4 - ], - "sim_method_num_steps": [ - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1, - 1 - ], - "tf": 2.5, - "time_steps": [ - 0.009765625, - 0.029296875, - 0.048828125, - 0.068359375, - 0.087890625, - 0.107421875, - 0.126953125, - 0.146484375, - 0.166015625, - 0.185546875, - 0.205078125, - 0.224609375, - 0.244140625, - 0.263671875, - 0.283203125, - 0.302734375 - ] - } -} \ No newline at end of file diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c deleted file mode 100644 index d0c541d3b..000000000 --- a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c +++ /dev/null @@ -1,278 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ -// standard -#include -#include - -// acados -#include "acados_c/external_function_interface.h" -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -#include "acados/sim/sim_common.h" -#include "acados/utils/external_function_generic.h" -#include "acados/utils/print.h" - - -// example specific -#include "lat_model/lat_model.h" -#include "acados_sim_solver_lat.h" - - -// ** solver data ** - -sim_solver_capsule * lat_acados_sim_solver_create_capsule() -{ - void* capsule_mem = malloc(sizeof(sim_solver_capsule)); - sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem; - - return capsule; -} - - -int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule) -{ - free(capsule); - return 0; -} - - -int lat_acados_sim_create(sim_solver_capsule * capsule) -{ - // initialize - const int nx = LAT_NX; - const int nu = LAT_NU; - const int nz = LAT_NZ; - const int np = LAT_NP; - bool tmp_bool; - - - double Tsim = 0.009765625; - - - // explicit ode - capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - - capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw; - capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in; - capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out; - capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in; - capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out; - capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work; - external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np); - - capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun; - capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in; - capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out; - capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in; - capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out; - capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work; - external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np); - - - - // sim plan & config - sim_solver_plan_t plan; - plan.sim_solver = ERK; - - // create correct config based on plan - sim_config * lat_sim_config = sim_config_create(plan); - capsule->acados_sim_config = lat_sim_config; - - // sim dims - void *lat_sim_dims = sim_dims_create(lat_sim_config); - capsule->acados_sim_dims = lat_sim_dims; - sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx); - sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu); - sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz); - - - // sim opts - sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims); - capsule->acados_sim_opts = lat_sim_opts; - int tmp_int = 3; - sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int); - sim_collocation_type collocation_type = GAUSS_LEGENDRE; - sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type); - - - tmp_int = 4; - sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int); - tmp_int = 1; - sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int); - tmp_bool = 0; - sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool); - - - // sim in / out - sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims); - capsule->acados_sim_in = lat_sim_in; - sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims); - capsule->acados_sim_out = lat_sim_out; - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "T", &Tsim); - - // model functions - lat_sim_config->model_set(lat_sim_in->model, - "expl_vde_for", capsule->sim_forw_vde_casadi); - lat_sim_config->model_set(lat_sim_in->model, - "expl_ode_fun", capsule->sim_expl_ode_fun_casadi); - - // sim solver - sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config, - lat_sim_dims, lat_sim_opts); - capsule->acados_sim_solver = lat_sim_solver; - - - /* initialize parameter values */ - double* p = calloc(np, sizeof(double)); - - - lat_acados_sim_update_params(capsule, p, np); - free(p); - - - /* initialize input */ - // x - double x0[4]; - for (int ii = 0; ii < 4; ii++) - x0[ii] = 0.0; - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "x", x0); - - - // u - double u0[1]; - for (int ii = 0; ii < 1; ii++) - u0[ii] = 0.0; - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "u", u0); - - // S_forw - double S_forw[20]; - for (int ii = 0; ii < 20; ii++) - S_forw[ii] = 0.0; - for (int ii = 0; ii < 4; ii++) - S_forw[ii + ii * 4 ] = 1.0; - - - sim_in_set(lat_sim_config, lat_sim_dims, - lat_sim_in, "S_forw", S_forw); - - int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out); - - return status; -} - - -int lat_acados_sim_solve(sim_solver_capsule *capsule) -{ - // integrate dynamics using acados sim_solver - int status = sim_solve(capsule->acados_sim_solver, - capsule->acados_sim_in, capsule->acados_sim_out); - if (status != 0) - printf("error in lat_acados_sim_solve()! Exiting.\n"); - - return status; -} - - -int lat_acados_sim_free(sim_solver_capsule *capsule) -{ - // free memory - sim_solver_destroy(capsule->acados_sim_solver); - sim_in_destroy(capsule->acados_sim_in); - sim_out_destroy(capsule->acados_sim_out); - sim_opts_destroy(capsule->acados_sim_opts); - sim_dims_destroy(capsule->acados_sim_dims); - sim_config_destroy(capsule->acados_sim_config); - - // free external function - external_function_param_casadi_free(capsule->sim_forw_vde_casadi); - external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi); - - return 0; -} - - -int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np) -{ - int status = 0; - int casadi_np = LAT_NP; - - if (casadi_np != np) { - printf("lat_acados_sim_update_params: trying to set %i parameters for external functions." - " External function has %i parameters. Exiting.\n", np, casadi_np); - exit(1); - } - capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p); - capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p); - - return status; -} - -/* getters pointers to C objects*/ -sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_config; -}; - -sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_in; -}; - -sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_out; -}; - -void * lat_acados_get_sim_dims(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_dims; -}; - -sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_opts; -}; - -sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule) -{ - return capsule->acados_sim_solver; -}; - diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h deleted file mode 100644 index 86b9c84c9..000000000 --- a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#ifndef ACADOS_SIM_lat_H_ -#define ACADOS_SIM_lat_H_ - -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -#define LAT_NX 4 -#define LAT_NZ 0 -#define LAT_NU 1 -#define LAT_NP 2 - -#ifdef __cplusplus -extern "C" { -#endif - - -// ** capsule for solver data ** -typedef struct sim_solver_capsule -{ - // acados objects - sim_in *acados_sim_in; - sim_out *acados_sim_out; - sim_solver *acados_sim_solver; - sim_opts *acados_sim_opts; - sim_config *acados_sim_config; - void *acados_sim_dims; - - /* external functions */ - // ERK - external_function_param_casadi * sim_forw_vde_casadi; - external_function_param_casadi * sim_expl_ode_fun_casadi; - external_function_param_casadi * sim_expl_ode_hess; - - // IRK - external_function_param_casadi * sim_impl_dae_fun; - external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z; - external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z; - external_function_param_casadi * sim_impl_dae_hess; - - // GNSF - external_function_param_casadi * sim_gnsf_phi_fun; - external_function_param_casadi * sim_gnsf_phi_fun_jac_y; - external_function_param_casadi * sim_gnsf_phi_jac_y_uhat; - external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z; - external_function_param_casadi * sim_gnsf_get_matrices_fun; - -} sim_solver_capsule; - - -ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np); - -ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule); -ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule); - - -ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void); -ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule); - -#ifdef __cplusplus -} -#endif - -#endif // ACADOS_SIM_lat_H_ diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c deleted file mode 100644 index 827b6c7f0..000000000 --- a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c +++ /dev/null @@ -1,264 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#define S_FUNCTION_NAME acados_solver_sfunction_lat -#define S_FUNCTION_LEVEL 2 - -#define MDL_START - -// acados -// #include "acados/utils/print.h" -#include "acados_c/sim_interface.h" -#include "acados_c/external_function_interface.h" - -// example specific -#include "lat_model/lat_model.h" -#include "acados_solver_lat.h" - -#include "simstruc.h" - -#define SAMPLINGTIME 0.009765625 - -static void mdlInitializeSizes (SimStruct *S) -{ - // specify the number of continuous and discrete states - ssSetNumContStates(S, 0); - ssSetNumDiscStates(S, 0);// specify the number of input ports - if ( !ssSetNumInputPorts(S, 8) ) - return; - - // specify the number of output ports - if ( !ssSetNumOutputPorts(S, 6) ) - return; - - // specify dimension information for the input ports - // lbx_0 - ssSetInputPortVectorDimension(S, 0, 4); - // ubx_0 - ssSetInputPortVectorDimension(S, 1, 4); - // parameters - ssSetInputPortVectorDimension(S, 2, (16+1) * 2); - // y_ref_0 - ssSetInputPortVectorDimension(S, 3, 3); - // y_ref - ssSetInputPortVectorDimension(S, 4, 45); - // y_ref_e - ssSetInputPortVectorDimension(S, 5, 2); - // lbx - ssSetInputPortVectorDimension(S, 6, 30); - // ubx - ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */ - ssSetOutputPortVectorDimension(S, 0, 1 ); - ssSetOutputPortVectorDimension(S, 1, 1 ); - ssSetOutputPortVectorDimension(S, 2, 1 ); - ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1 - ssSetOutputPortVectorDimension(S, 4, 1); - ssSetOutputPortVectorDimension(S, 5, 1 ); - - // specify the direct feedthrough status - // should be set to 1 for all inputs used in mdlOutputs - ssSetInputPortDirectFeedThrough(S, 0, 1); - ssSetInputPortDirectFeedThrough(S, 1, 1); - ssSetInputPortDirectFeedThrough(S, 2, 1); - ssSetInputPortDirectFeedThrough(S, 3, 1); - ssSetInputPortDirectFeedThrough(S, 4, 1); - ssSetInputPortDirectFeedThrough(S, 5, 1); - ssSetInputPortDirectFeedThrough(S, 6, 1); - ssSetInputPortDirectFeedThrough(S, 7, 1); - - // one sample time - ssSetNumSampleTimes(S, 1); -} - - -#if defined(MATLAB_MEX_FILE) - -#define MDL_SET_INPUT_PORT_DIMENSION_INFO -#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO - -static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) -{ - if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) ) - return; -} - -static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo) -{ - if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) ) - return; -} - -#endif /* MATLAB_MEX_FILE */ - - -static void mdlInitializeSampleTimes(SimStruct *S) -{ - ssSetSampleTime(S, 0, SAMPLINGTIME); - ssSetOffsetTime(S, 0, 0.0); -} - - -static void mdlStart(SimStruct *S) -{ - lat_solver_capsule *capsule = lat_acados_create_capsule(); - lat_acados_create(capsule); - - ssSetUserData(S, (void*)capsule); -} - - -static void mdlOutputs(SimStruct *S, int_T tid) -{ - lat_solver_capsule *capsule = ssGetUserData(S); - ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule); - ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule); - ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule); - ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule); - - InputRealPtrsType in_sign; - - // local buffer - real_t buffer[4]; - - /* go through inputs */ - // lbx_0 - in_sign = ssGetInputPortRealSignalPtrs(S, 0); - for (int i = 0; i < 4; i++) - buffer[i] = (double)(*in_sign[i]); - - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer); - // ubx_0 - in_sign = ssGetInputPortRealSignalPtrs(S, 1); - for (int i = 0; i < 4; i++) - buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer); - // parameters - stage-variant !!! - in_sign = ssGetInputPortRealSignalPtrs(S, 2); - - // update value of parameters - for (int ii = 0; ii <= 16; ii++) - { - for (int jj = 0; jj < 2; jj++) - buffer[jj] = (double)(*in_sign[ii*2+jj]); - lat_acados_update_params(capsule, ii, buffer, 2); - } - - - // y_ref_0 - in_sign = ssGetInputPortRealSignalPtrs(S, 3); - - for (int i = 0; i < 3; i++) - buffer[i] = (double)(*in_sign[i]); - - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer); - - - // y_ref - for stages 1 to N-1 - in_sign = ssGetInputPortRealSignalPtrs(S, 4); - - for (int ii = 1; ii < 16; ii++) - { - for (int jj = 0; jj < 3; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer); - } - - - // y_ref_e - in_sign = ssGetInputPortRealSignalPtrs(S, 5); - - for (int i = 0; i < 2; i++) - buffer[i] = (double)(*in_sign[i]); - - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer); - // lbx - in_sign = ssGetInputPortRealSignalPtrs(S, 6); - for (int ii = 1; ii < 16; ii++) - { - for (int jj = 0; jj < 2; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer); - } - // ubx - in_sign = ssGetInputPortRealSignalPtrs(S, 7); - for (int ii = 1; ii < 16; ii++) - { - for (int jj = 0; jj < 2; jj++) - buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer); - } - - /* call solver */ - int rti_phase = 0; - ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase); - int acados_status = lat_acados_solve(capsule); - - - /* set outputs */ - // assign pointers to output signals - real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin; - int tmp_int; - out_u0 = ssGetOutputPortRealSignal(S, 0); - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0); - - - out_status = ssGetOutputPortRealSignal(S, 1); - *out_status = (real_t) acados_status; - out_KKT_res = ssGetOutputPortRealSignal(S, 2); - *out_KKT_res = (real_t) nlp_out->inf_norm_res; - out_x1 = ssGetOutputPortRealSignal(S, 3); - ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1); - out_cpu_time = ssGetOutputPortRealSignal(S, 4); - // get solution time - ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time); - out_sqp_iter = ssGetOutputPortRealSignal(S, 5); - // get sqp iter - ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int); - *out_sqp_iter = (real_t) tmp_int; - -} - -static void mdlTerminate(SimStruct *S) -{ - lat_solver_capsule *capsule = ssGetUserData(S); - - lat_acados_free(capsule); - lat_acados_free_capsule(capsule); -} - - -#ifdef MATLAB_MEX_FILE -#include "simulink.c" -#else -#include "cg_sfun.h" -#endif diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c deleted file mode 100644 index c536654fd..000000000 --- a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c +++ /dev/null @@ -1,128 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -// standard -#include -#include -// acados -#include "acados/utils/print.h" -#include "acados/utils/math.h" -#include "acados_c/sim_interface.h" -#include "acados_sim_solver_lat.h" - -#define NX LAT_NX -#define NZ LAT_NZ -#define NU LAT_NU -#define NP LAT_NP - - -int main() -{ - int status = 0; - sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule(); - status = lat_acados_sim_create(capsule); - - if (status) - { - printf("acados_create() returned status %d. Exiting.\n", status); - exit(1); - } - - sim_config *acados_sim_config = lat_acados_get_sim_config(capsule); - sim_in *acados_sim_in = lat_acados_get_sim_in(capsule); - sim_out *acados_sim_out = lat_acados_get_sim_out(capsule); - void *acados_sim_dims = lat_acados_get_sim_dims(capsule); - - // initial condition - double x_current[NX]; - x_current[0] = 0.0; - x_current[1] = 0.0; - x_current[2] = 0.0; - x_current[3] = 0.0; - - - x_current[0] = 0; - x_current[1] = 0; - x_current[2] = 0; - x_current[3] = 0; - - - - - // initial value for control input - double u0[NU]; - u0[0] = 0.0; - // set parameters - double p[NP]; - p[0] = 0; - p[1] = 0; - - lat_acados_sim_update_params(capsule, p, NP); - - - int n_sim_steps = 3; - // solve ocp in loop - for (int ii = 0; ii < n_sim_steps; ii++) - { - sim_in_set(acados_sim_config, acados_sim_dims, - acados_sim_in, "x", x_current); - status = lat_acados_sim_solve(capsule); - - if (status != ACADOS_SUCCESS) - { - printf("acados_solve() failed with status %d.\n", status); - } - - sim_out_get(acados_sim_config, acados_sim_dims, - acados_sim_out, "x", x_current); - - printf("\nx_current, %d\n", ii); - for (int jj = 0; jj < NX; jj++) - { - printf("%e\n", x_current[jj]); - } - } - - printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps); - - // free solver - status = lat_acados_sim_free(capsule); - if (status) { - printf("lat_acados_sim_free() returned status %d. \n", status); - } - - lat_acados_sim_solver_free_capsule(capsule); - - return status; -} diff --git a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m b/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m deleted file mode 100644 index 2c6679ecb..000000000 --- a/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m +++ /dev/null @@ -1,125 +0,0 @@ -% -% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, -% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, -% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, -% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl -% -% This file is part of acados. -% -% The 2-Clause BSD License -% -% Redistribution and use in source and binary forms, with or without -% modification, are permitted provided that the following conditions are met: -% -% 1. Redistributions of source code must retain the above copyright notice, -% this list of conditions and the following disclaimer. -% -% 2. Redistributions in binary form must reproduce the above copyright notice, -% this list of conditions and the following disclaimer in the documentation -% and/or other materials provided with the distribution. -% -% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -% POSSIBILITY OF SUCH DAMAGE.; -% - -SOURCES = { ... - 'lat_model/lat_expl_ode_fun.c', ... - 'lat_model/lat_expl_vde_forw.c',... - 'lat_cost/lat_cost_y_0_fun.c',... - 'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',... - 'lat_cost/lat_cost_y_0_hess.c',... - 'lat_cost/lat_cost_y_fun.c',... - 'lat_cost/lat_cost_y_fun_jac_ut_xt.c',... - 'lat_cost/lat_cost_y_hess.c',... - 'lat_cost/lat_cost_y_e_fun.c',... - 'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',... - 'lat_cost/lat_cost_y_e_hess.c',... - 'acados_solver_sfunction_lat.c', ... - 'acados_solver_lat.c' - }; - -INC_PATH = '/data/openpilot/third_party/acados/include/acados/include'; - -INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ... - ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ... - ['-I', fullfile(INC_PATH, 'acados')], ... - ['-I', fullfile(INC_PATH)]}; - - - -CFLAGS = 'CFLAGS=$CFLAGS'; -LDFLAGS = 'LDFLAGS=$LDFLAGS'; -COMPFLAGS = 'COMPFLAGS=$COMPFLAGS'; -COMPDEFINES = 'COMPDEFINES=$COMPDEFINES'; - - - -LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')]; - -LIBS = {'-lacados', '-lhpipm', '-lblasfeo'}; - -% acados linking libraries and flags - - -mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ... - LIB_PATH, LIBS{:}, SOURCES{:}, ... - '-output', 'acados_solver_sfunction_lat' ); - -fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ... - eval('mexext')] ); - - -%% print note on usage of s-function -fprintf('\n\nNote: Usage of Sfunction is as follows:\n') -input_note = 'Inputs are:\n'; -i_in = 1; -input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',... - ' size [4]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',... - ' size [4]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',... - ' size [34]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',... - ' size [45]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n '); -i_in = i_in + 1; -input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n '); -i_in = i_in + 1; - -fprintf(input_note) - -disp(' ') - -output_note = 'Outputs are:\n'; -i_out = 0; -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') KKT residual\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') CPU time\n '); -i_out = i_out + 1; -output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n '); - -fprintf(output_note) diff --git a/selfdrive/locationd/locationd b/selfdrive/locationd/locationd index 2c3c2f9ae..7093f054b 100755 Binary files a/selfdrive/locationd/locationd and b/selfdrive/locationd/locationd differ diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 7d82e9ecd..1b9cab977 100644 --- a/selfdrive/locationd/models/generated/car.cpp +++ b/selfdrive/locationd/models/generated/car.cpp @@ -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_8581923876106076715) { - out_8581923876106076715[0] = delta_x[0] + nom_x[0]; - out_8581923876106076715[1] = delta_x[1] + nom_x[1]; - out_8581923876106076715[2] = delta_x[2] + nom_x[2]; - out_8581923876106076715[3] = delta_x[3] + nom_x[3]; - out_8581923876106076715[4] = delta_x[4] + nom_x[4]; - out_8581923876106076715[5] = delta_x[5] + nom_x[5]; - out_8581923876106076715[6] = delta_x[6] + nom_x[6]; - out_8581923876106076715[7] = delta_x[7] + nom_x[7]; - out_8581923876106076715[8] = delta_x[8] + nom_x[8]; +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 inv_err_fun(double *nom_x, double *true_x, double *out_4891521151268278961) { - out_4891521151268278961[0] = -nom_x[0] + true_x[0]; - out_4891521151268278961[1] = -nom_x[1] + true_x[1]; - out_4891521151268278961[2] = -nom_x[2] + true_x[2]; - out_4891521151268278961[3] = -nom_x[3] + true_x[3]; - out_4891521151268278961[4] = -nom_x[4] + true_x[4]; - out_4891521151268278961[5] = -nom_x[5] + true_x[5]; - out_4891521151268278961[6] = -nom_x[6] + true_x[6]; - out_4891521151268278961[7] = -nom_x[7] + true_x[7]; - out_4891521151268278961[8] = -nom_x[8] + true_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 H_mod_fun(double *state, double *out_6523549794488946483) { - out_6523549794488946483[0] = 1.0; - out_6523549794488946483[1] = 0; - out_6523549794488946483[2] = 0; - out_6523549794488946483[3] = 0; - out_6523549794488946483[4] = 0; - out_6523549794488946483[5] = 0; - out_6523549794488946483[6] = 0; - out_6523549794488946483[7] = 0; - out_6523549794488946483[8] = 0; - out_6523549794488946483[9] = 0; - out_6523549794488946483[10] = 1.0; - out_6523549794488946483[11] = 0; - out_6523549794488946483[12] = 0; - out_6523549794488946483[13] = 0; - out_6523549794488946483[14] = 0; - out_6523549794488946483[15] = 0; - out_6523549794488946483[16] = 0; - out_6523549794488946483[17] = 0; - out_6523549794488946483[18] = 0; - out_6523549794488946483[19] = 0; - out_6523549794488946483[20] = 1.0; - out_6523549794488946483[21] = 0; - out_6523549794488946483[22] = 0; - out_6523549794488946483[23] = 0; - out_6523549794488946483[24] = 0; - out_6523549794488946483[25] = 0; - out_6523549794488946483[26] = 0; - out_6523549794488946483[27] = 0; - out_6523549794488946483[28] = 0; - out_6523549794488946483[29] = 0; - out_6523549794488946483[30] = 1.0; - out_6523549794488946483[31] = 0; - out_6523549794488946483[32] = 0; - out_6523549794488946483[33] = 0; - out_6523549794488946483[34] = 0; - out_6523549794488946483[35] = 0; - out_6523549794488946483[36] = 0; - out_6523549794488946483[37] = 0; - out_6523549794488946483[38] = 0; - out_6523549794488946483[39] = 0; - out_6523549794488946483[40] = 1.0; - out_6523549794488946483[41] = 0; - out_6523549794488946483[42] = 0; - out_6523549794488946483[43] = 0; - out_6523549794488946483[44] = 0; - out_6523549794488946483[45] = 0; - out_6523549794488946483[46] = 0; - out_6523549794488946483[47] = 0; - out_6523549794488946483[48] = 0; - out_6523549794488946483[49] = 0; - out_6523549794488946483[50] = 1.0; - out_6523549794488946483[51] = 0; - out_6523549794488946483[52] = 0; - out_6523549794488946483[53] = 0; - out_6523549794488946483[54] = 0; - out_6523549794488946483[55] = 0; - out_6523549794488946483[56] = 0; - out_6523549794488946483[57] = 0; - out_6523549794488946483[58] = 0; - out_6523549794488946483[59] = 0; - out_6523549794488946483[60] = 1.0; - out_6523549794488946483[61] = 0; - out_6523549794488946483[62] = 0; - out_6523549794488946483[63] = 0; - out_6523549794488946483[64] = 0; - out_6523549794488946483[65] = 0; - out_6523549794488946483[66] = 0; - out_6523549794488946483[67] = 0; - out_6523549794488946483[68] = 0; - out_6523549794488946483[69] = 0; - out_6523549794488946483[70] = 1.0; - out_6523549794488946483[71] = 0; - out_6523549794488946483[72] = 0; - out_6523549794488946483[73] = 0; - out_6523549794488946483[74] = 0; - out_6523549794488946483[75] = 0; - out_6523549794488946483[76] = 0; - out_6523549794488946483[77] = 0; - out_6523549794488946483[78] = 0; - out_6523549794488946483[79] = 0; - out_6523549794488946483[80] = 1.0; +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 f_fun(double *state, double dt, double *out_3111820312432597939) { - out_3111820312432597939[0] = state[0]; - out_3111820312432597939[1] = state[1]; - out_3111820312432597939[2] = state[2]; - out_3111820312432597939[3] = state[3]; - out_3111820312432597939[4] = state[4]; - out_3111820312432597939[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_3111820312432597939[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_3111820312432597939[7] = state[7]; - out_3111820312432597939[8] = state[8]; +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_5596968740727191559) { - out_5596968740727191559[0] = 1; - out_5596968740727191559[1] = 0; - out_5596968740727191559[2] = 0; - out_5596968740727191559[3] = 0; - out_5596968740727191559[4] = 0; - out_5596968740727191559[5] = 0; - out_5596968740727191559[6] = 0; - out_5596968740727191559[7] = 0; - out_5596968740727191559[8] = 0; - out_5596968740727191559[9] = 0; - out_5596968740727191559[10] = 1; - out_5596968740727191559[11] = 0; - out_5596968740727191559[12] = 0; - out_5596968740727191559[13] = 0; - out_5596968740727191559[14] = 0; - out_5596968740727191559[15] = 0; - out_5596968740727191559[16] = 0; - out_5596968740727191559[17] = 0; - out_5596968740727191559[18] = 0; - out_5596968740727191559[19] = 0; - out_5596968740727191559[20] = 1; - out_5596968740727191559[21] = 0; - out_5596968740727191559[22] = 0; - out_5596968740727191559[23] = 0; - out_5596968740727191559[24] = 0; - out_5596968740727191559[25] = 0; - out_5596968740727191559[26] = 0; - out_5596968740727191559[27] = 0; - out_5596968740727191559[28] = 0; - out_5596968740727191559[29] = 0; - out_5596968740727191559[30] = 1; - out_5596968740727191559[31] = 0; - out_5596968740727191559[32] = 0; - out_5596968740727191559[33] = 0; - out_5596968740727191559[34] = 0; - out_5596968740727191559[35] = 0; - out_5596968740727191559[36] = 0; - out_5596968740727191559[37] = 0; - out_5596968740727191559[38] = 0; - out_5596968740727191559[39] = 0; - out_5596968740727191559[40] = 1; - out_5596968740727191559[41] = 0; - out_5596968740727191559[42] = 0; - out_5596968740727191559[43] = 0; - out_5596968740727191559[44] = 0; - out_5596968740727191559[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_5596968740727191559[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_5596968740727191559[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_5596968740727191559[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_5596968740727191559[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_5596968740727191559[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_5596968740727191559[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_5596968740727191559[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_5596968740727191559[53] = -9.8000000000000007*dt; - out_5596968740727191559[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_5596968740727191559[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_5596968740727191559[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_5596968740727191559[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_5596968740727191559[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_5596968740727191559[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_5596968740727191559[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_5596968740727191559[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_5596968740727191559[62] = 0; - out_5596968740727191559[63] = 0; - out_5596968740727191559[64] = 0; - out_5596968740727191559[65] = 0; - out_5596968740727191559[66] = 0; - out_5596968740727191559[67] = 0; - out_5596968740727191559[68] = 0; - out_5596968740727191559[69] = 0; - out_5596968740727191559[70] = 1; - out_5596968740727191559[71] = 0; - out_5596968740727191559[72] = 0; - out_5596968740727191559[73] = 0; - out_5596968740727191559[74] = 0; - out_5596968740727191559[75] = 0; - out_5596968740727191559[76] = 0; - out_5596968740727191559[77] = 0; - out_5596968740727191559[78] = 0; - out_5596968740727191559[79] = 0; - out_5596968740727191559[80] = 1; +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 h_25(double *state, double *unused, double *out_8255502319032992897) { - out_8255502319032992897[0] = state[6]; +void h_25(double *state, double *unused, double *out_579481060641732431) { + out_579481060641732431[0] = state[6]; } -void H_25(double *state, double *unused, double *out_8118118653173567889) { - out_8118118653173567889[0] = 0; - out_8118118653173567889[1] = 0; - out_8118118653173567889[2] = 0; - out_8118118653173567889[3] = 0; - out_8118118653173567889[4] = 0; - out_8118118653173567889[5] = 0; - out_8118118653173567889[6] = 1; - out_8118118653173567889[7] = 0; - out_8118118653173567889[8] = 0; +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_24(double *state, double *unused, double *out_5005612245441996689) { - out_5005612245441996689[0] = state[4]; - out_5005612245441996689[1] = state[5]; +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_5892410869194699327) { - out_5892410869194699327[0] = 0; - out_5892410869194699327[1] = 0; - out_5892410869194699327[2] = 0; - out_5892410869194699327[3] = 0; - out_5892410869194699327[4] = 1; - out_5892410869194699327[5] = 0; - out_5892410869194699327[6] = 0; - out_5892410869194699327[7] = 0; - out_5892410869194699327[8] = 0; - out_5892410869194699327[9] = 0; - out_5892410869194699327[10] = 0; - out_5892410869194699327[11] = 0; - out_5892410869194699327[12] = 0; - out_5892410869194699327[13] = 0; - out_5892410869194699327[14] = 1; - out_5892410869194699327[15] = 0; - out_5892410869194699327[16] = 0; - out_5892410869194699327[17] = 0; +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_30(double *state, double *unused, double *out_3420406528326207783) { - out_3420406528326207783[0] = state[4]; +void h_30(double *state, double *unused, double *out_8060396416076809453) { + out_8060396416076809453[0] = state[4]; } -void H_30(double *state, double *unused, double *out_1201428311681951134) { - out_1201428311681951134[0] = 0; - out_1201428311681951134[1] = 0; - out_1201428311681951134[2] = 0; - out_1201428311681951134[3] = 0; - out_1201428311681951134[4] = 1; - out_1201428311681951134[5] = 0; - out_1201428311681951134[6] = 0; - out_1201428311681951134[7] = 0; - out_1201428311681951134[8] = 0; +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_26(double *state, double *unused, double *out_1805500687509117164) { - out_1805500687509117164[0] = state[7]; +void h_26(double *state, double *unused, double *out_8663712300753286970) { + out_8663712300753286970[0] = state[7]; } -void H_26(double *state, double *unused, double *out_6587122101661927503) { - out_6587122101661927503[0] = 0; - out_6587122101661927503[1] = 0; - out_6587122101661927503[2] = 0; - out_6587122101661927503[3] = 0; - out_6587122101661927503[4] = 0; - out_6587122101661927503[5] = 0; - out_6587122101661927503[6] = 0; - out_6587122101661927503[7] = 1; - out_6587122101661927503[8] = 0; +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_27(double *state, double *unused, double *out_9023497953600416982) { - out_9023497953600416982[0] = state[3]; +void h_27(double *state, double *unused, double *out_2233071042628120952) { + out_2233071042628120952[0] = state[3]; } -void H_27(double *state, double *unused, double *out_3376191623482376045) { - out_3376191623482376045[0] = 0; - out_3376191623482376045[1] = 0; - out_3376191623482376045[2] = 0; - out_3376191623482376045[3] = 1; - out_3376191623482376045[4] = 0; - out_3376191623482376045[5] = 0; - out_3376191623482376045[6] = 0; - out_3376191623482376045[7] = 0; - out_3376191623482376045[8] = 0; +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_29(double *state, double *unused, double *out_9148052057824628745) { - out_9148052057824628745[0] = state[1]; +void h_29(double *state, double *unused, double *out_8528880318658063037) { + out_8528880318658063037[0] = state[1]; } -void H_29(double *state, double *unused, double *out_691196967367558950) { - out_691196967367558950[0] = 0; - out_691196967367558950[1] = 1; - out_691196967367558950[2] = 0; - out_691196967367558950[3] = 0; - out_691196967367558950[4] = 0; - out_691196967367558950[5] = 0; - out_691196967367558950[6] = 0; - out_691196967367558950[7] = 0; - out_691196967367558950[8] = 0; +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_28(double *state, double *unused, double *out_402702707668658192) { - out_402702707668658192[0] = state[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_5773595984437089524) { - out_5773595984437089524[0] = 1; - out_5773595984437089524[1] = 0; - out_5773595984437089524[2] = 0; - out_5773595984437089524[3] = 0; - out_5773595984437089524[4] = 0; - out_5773595984437089524[5] = 0; - out_5773595984437089524[6] = 0; - out_5773595984437089524[7] = 0; - out_5773595984437089524[8] = 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_31(double *state, double *unused, double *out_6661654130949307838) { - out_6661654130949307838[0] = state[8]; +void h_31(double *state, double *unused, double *out_7350316286992083367) { + out_7350316286992083367[0] = state[8]; } -void H_31(double *state, double *unused, double *out_8087472691296607461) { - out_8087472691296607461[0] = 0; - out_8087472691296607461[1] = 0; - out_8087472691296607461[2] = 0; - out_8087472691296607461[3] = 0; - out_8087472691296607461[4] = 0; - out_8087472691296607461[5] = 0; - out_8087472691296607461[6] = 0; - out_8087472691296607461[7] = 0; - out_8087472691296607461[8] = 1; +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; } #include #include @@ -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_8581923876106076715) { - err_fun(nom_x, delta_x, out_8581923876106076715); +void car_err_fun(double *nom_x, double *delta_x, double *out_1840749225442711100) { + err_fun(nom_x, delta_x, out_1840749225442711100); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4891521151268278961) { - inv_err_fun(nom_x, true_x, out_4891521151268278961); +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_H_mod_fun(double *state, double *out_6523549794488946483) { - H_mod_fun(state, out_6523549794488946483); +void car_H_mod_fun(double *state, double *out_5849575757077882696) { + H_mod_fun(state, out_5849575757077882696); } -void car_f_fun(double *state, double dt, double *out_3111820312432597939) { - f_fun(state, dt, out_3111820312432597939); +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_5596968740727191559) { - F_fun(state, dt, out_5596968740727191559); +void car_F_fun(double *state, double dt, double *out_1015750461961158607) { + F_fun(state, dt, out_1015750461961158607); } -void car_h_25(double *state, double *unused, double *out_8255502319032992897) { - h_25(state, unused, out_8255502319032992897); +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_8118118653173567889) { - H_25(state, unused, out_8118118653173567889); +void car_H_25(double *state, double *unused, double *out_8838814511200069080) { + H_25(state, unused, out_8838814511200069080); } -void car_h_24(double *state, double *unused, double *out_5005612245441996689) { - h_24(state, unused, out_5005612245441996689); +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_5892410869194699327) { - H_24(state, unused, out_5892410869194699327); +void car_H_24(double *state, double *unused, double *out_7435279963503982970) { + H_24(state, unused, out_7435279963503982970); } -void car_h_30(double *state, double *unused, double *out_3420406528326207783) { - h_30(state, unused, out_3420406528326207783); +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_1201428311681951134) { - H_30(state, unused, out_1201428311681951134); +void car_H_30(double *state, double *unused, double *out_6320481552692820453) { + H_30(state, unused, out_6320481552692820453); } -void car_h_26(double *state, double *unused, double *out_1805500687509117164) { - h_26(state, unused, out_1805500687509117164); +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_6587122101661927503) { - H_26(state, unused, out_6587122101661927503); +void car_H_26(double *state, double *unused, double *out_5866426243635426312) { + H_26(state, unused, out_5866426243635426312); } -void car_h_27(double *state, double *unused, double *out_9023497953600416982) { - h_27(state, unused, out_9023497953600416982); +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_3376191623482376045) { - H_27(state, unused, out_3376191623482376045); +void car_H_27(double *state, double *unused, double *out_4096887481508877236) { + H_27(state, unused, out_4096887481508877236); } -void car_h_29(double *state, double *unused, double *out_9148052057824628745) { - h_29(state, unused, out_9148052057824628745); +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_691196967367558950) { - H_29(state, unused, out_691196967367558950); +void car_H_29(double *state, double *unused, double *out_5810250208378428269) { + H_29(state, unused, out_5810250208378428269); } -void car_h_28(double *state, double *unused, double *out_402702707668658192) { - h_28(state, unused, out_402702707668658192); +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_5773595984437089524) { - H_28(state, unused, out_5773595984437089524); +void car_H_28(double *state, double *unused, double *out_7554094848261592773) { + H_28(state, unused, out_7554094848261592773); } -void car_h_31(double *state, double *unused, double *out_6661654130949307838) { - h_31(state, unused, out_6661654130949307838); +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_8087472691296607461) { - H_31(state, unused, out_8087472691296607461); +void car_H_31(double *state, double *unused, double *out_5240218141402074836) { + H_31(state, unused, out_5240218141402074836); } void car_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h index 1aa9a62a6..908149833 100644 --- a/selfdrive/locationd/models/generated/car.h +++ b/selfdrive/locationd/models/generated/car.h @@ -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_8581923876106076715); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4891521151268278961); -void car_H_mod_fun(double *state, double *out_6523549794488946483); -void car_f_fun(double *state, double dt, double *out_3111820312432597939); -void car_F_fun(double *state, double dt, double *out_5596968740727191559); -void car_h_25(double *state, double *unused, double *out_8255502319032992897); -void car_H_25(double *state, double *unused, double *out_8118118653173567889); -void car_h_24(double *state, double *unused, double *out_5005612245441996689); -void car_H_24(double *state, double *unused, double *out_5892410869194699327); -void car_h_30(double *state, double *unused, double *out_3420406528326207783); -void car_H_30(double *state, double *unused, double *out_1201428311681951134); -void car_h_26(double *state, double *unused, double *out_1805500687509117164); -void car_H_26(double *state, double *unused, double *out_6587122101661927503); -void car_h_27(double *state, double *unused, double *out_9023497953600416982); -void car_H_27(double *state, double *unused, double *out_3376191623482376045); -void car_h_29(double *state, double *unused, double *out_9148052057824628745); -void car_H_29(double *state, double *unused, double *out_691196967367558950); -void car_h_28(double *state, double *unused, double *out_402702707668658192); -void car_H_28(double *state, double *unused, double *out_5773595984437089524); -void car_h_31(double *state, double *unused, double *out_6661654130949307838); -void car_H_31(double *state, double *unused, double *out_8087472691296607461); +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_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); diff --git a/selfdrive/locationd/models/generated/gnss.cpp b/selfdrive/locationd/models/generated/gnss.cpp index 607b62872..bc1f786a8 100644 --- a/selfdrive/locationd/models/generated/gnss.cpp +++ b/selfdrive/locationd/models/generated/gnss.cpp @@ -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_6740791193231733360) { - out_6740791193231733360[0] = delta_x[0] + nom_x[0]; - out_6740791193231733360[1] = delta_x[1] + nom_x[1]; - out_6740791193231733360[2] = delta_x[2] + nom_x[2]; - out_6740791193231733360[3] = delta_x[3] + nom_x[3]; - out_6740791193231733360[4] = delta_x[4] + nom_x[4]; - out_6740791193231733360[5] = delta_x[5] + nom_x[5]; - out_6740791193231733360[6] = delta_x[6] + nom_x[6]; - out_6740791193231733360[7] = delta_x[7] + nom_x[7]; - out_6740791193231733360[8] = delta_x[8] + nom_x[8]; - out_6740791193231733360[9] = delta_x[9] + nom_x[9]; - out_6740791193231733360[10] = delta_x[10] + nom_x[10]; +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 inv_err_fun(double *nom_x, double *true_x, double *out_5435998030533772419) { - out_5435998030533772419[0] = -nom_x[0] + true_x[0]; - out_5435998030533772419[1] = -nom_x[1] + true_x[1]; - out_5435998030533772419[2] = -nom_x[2] + true_x[2]; - out_5435998030533772419[3] = -nom_x[3] + true_x[3]; - out_5435998030533772419[4] = -nom_x[4] + true_x[4]; - out_5435998030533772419[5] = -nom_x[5] + true_x[5]; - out_5435998030533772419[6] = -nom_x[6] + true_x[6]; - out_5435998030533772419[7] = -nom_x[7] + true_x[7]; - out_5435998030533772419[8] = -nom_x[8] + true_x[8]; - out_5435998030533772419[9] = -nom_x[9] + true_x[9]; - out_5435998030533772419[10] = -nom_x[10] + true_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 H_mod_fun(double *state, double *out_8750904480143678209) { - out_8750904480143678209[0] = 1.0; - out_8750904480143678209[1] = 0; - out_8750904480143678209[2] = 0; - out_8750904480143678209[3] = 0; - out_8750904480143678209[4] = 0; - out_8750904480143678209[5] = 0; - out_8750904480143678209[6] = 0; - out_8750904480143678209[7] = 0; - out_8750904480143678209[8] = 0; - out_8750904480143678209[9] = 0; - out_8750904480143678209[10] = 0; - out_8750904480143678209[11] = 0; - out_8750904480143678209[12] = 1.0; - out_8750904480143678209[13] = 0; - out_8750904480143678209[14] = 0; - out_8750904480143678209[15] = 0; - out_8750904480143678209[16] = 0; - out_8750904480143678209[17] = 0; - out_8750904480143678209[18] = 0; - out_8750904480143678209[19] = 0; - out_8750904480143678209[20] = 0; - out_8750904480143678209[21] = 0; - out_8750904480143678209[22] = 0; - out_8750904480143678209[23] = 0; - out_8750904480143678209[24] = 1.0; - out_8750904480143678209[25] = 0; - out_8750904480143678209[26] = 0; - out_8750904480143678209[27] = 0; - out_8750904480143678209[28] = 0; - out_8750904480143678209[29] = 0; - out_8750904480143678209[30] = 0; - out_8750904480143678209[31] = 0; - out_8750904480143678209[32] = 0; - out_8750904480143678209[33] = 0; - out_8750904480143678209[34] = 0; - out_8750904480143678209[35] = 0; - out_8750904480143678209[36] = 1.0; - out_8750904480143678209[37] = 0; - out_8750904480143678209[38] = 0; - out_8750904480143678209[39] = 0; - out_8750904480143678209[40] = 0; - out_8750904480143678209[41] = 0; - out_8750904480143678209[42] = 0; - out_8750904480143678209[43] = 0; - out_8750904480143678209[44] = 0; - out_8750904480143678209[45] = 0; - out_8750904480143678209[46] = 0; - out_8750904480143678209[47] = 0; - out_8750904480143678209[48] = 1.0; - out_8750904480143678209[49] = 0; - out_8750904480143678209[50] = 0; - out_8750904480143678209[51] = 0; - out_8750904480143678209[52] = 0; - out_8750904480143678209[53] = 0; - out_8750904480143678209[54] = 0; - out_8750904480143678209[55] = 0; - out_8750904480143678209[56] = 0; - out_8750904480143678209[57] = 0; - out_8750904480143678209[58] = 0; - out_8750904480143678209[59] = 0; - out_8750904480143678209[60] = 1.0; - out_8750904480143678209[61] = 0; - out_8750904480143678209[62] = 0; - out_8750904480143678209[63] = 0; - out_8750904480143678209[64] = 0; - out_8750904480143678209[65] = 0; - out_8750904480143678209[66] = 0; - out_8750904480143678209[67] = 0; - out_8750904480143678209[68] = 0; - out_8750904480143678209[69] = 0; - out_8750904480143678209[70] = 0; - out_8750904480143678209[71] = 0; - out_8750904480143678209[72] = 1.0; - out_8750904480143678209[73] = 0; - out_8750904480143678209[74] = 0; - out_8750904480143678209[75] = 0; - out_8750904480143678209[76] = 0; - out_8750904480143678209[77] = 0; - out_8750904480143678209[78] = 0; - out_8750904480143678209[79] = 0; - out_8750904480143678209[80] = 0; - out_8750904480143678209[81] = 0; - out_8750904480143678209[82] = 0; - out_8750904480143678209[83] = 0; - out_8750904480143678209[84] = 1.0; - out_8750904480143678209[85] = 0; - out_8750904480143678209[86] = 0; - out_8750904480143678209[87] = 0; - out_8750904480143678209[88] = 0; - out_8750904480143678209[89] = 0; - out_8750904480143678209[90] = 0; - out_8750904480143678209[91] = 0; - out_8750904480143678209[92] = 0; - out_8750904480143678209[93] = 0; - out_8750904480143678209[94] = 0; - out_8750904480143678209[95] = 0; - out_8750904480143678209[96] = 1.0; - out_8750904480143678209[97] = 0; - out_8750904480143678209[98] = 0; - out_8750904480143678209[99] = 0; - out_8750904480143678209[100] = 0; - out_8750904480143678209[101] = 0; - out_8750904480143678209[102] = 0; - out_8750904480143678209[103] = 0; - out_8750904480143678209[104] = 0; - out_8750904480143678209[105] = 0; - out_8750904480143678209[106] = 0; - out_8750904480143678209[107] = 0; - out_8750904480143678209[108] = 1.0; - out_8750904480143678209[109] = 0; - out_8750904480143678209[110] = 0; - out_8750904480143678209[111] = 0; - out_8750904480143678209[112] = 0; - out_8750904480143678209[113] = 0; - out_8750904480143678209[114] = 0; - out_8750904480143678209[115] = 0; - out_8750904480143678209[116] = 0; - out_8750904480143678209[117] = 0; - out_8750904480143678209[118] = 0; - out_8750904480143678209[119] = 0; - out_8750904480143678209[120] = 1.0; +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 f_fun(double *state, double dt, double *out_571033032076185663) { - out_571033032076185663[0] = dt*state[3] + state[0]; - out_571033032076185663[1] = dt*state[4] + state[1]; - out_571033032076185663[2] = dt*state[5] + state[2]; - out_571033032076185663[3] = state[3]; - out_571033032076185663[4] = state[4]; - out_571033032076185663[5] = state[5]; - out_571033032076185663[6] = dt*state[7] + state[6]; - out_571033032076185663[7] = dt*state[8] + state[7]; - out_571033032076185663[8] = state[8]; - out_571033032076185663[9] = state[9]; - out_571033032076185663[10] = state[10]; +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_8071961880976414619) { - out_8071961880976414619[0] = 1; - out_8071961880976414619[1] = 0; - out_8071961880976414619[2] = 0; - out_8071961880976414619[3] = dt; - out_8071961880976414619[4] = 0; - out_8071961880976414619[5] = 0; - out_8071961880976414619[6] = 0; - out_8071961880976414619[7] = 0; - out_8071961880976414619[8] = 0; - out_8071961880976414619[9] = 0; - out_8071961880976414619[10] = 0; - out_8071961880976414619[11] = 0; - out_8071961880976414619[12] = 1; - out_8071961880976414619[13] = 0; - out_8071961880976414619[14] = 0; - out_8071961880976414619[15] = dt; - out_8071961880976414619[16] = 0; - out_8071961880976414619[17] = 0; - out_8071961880976414619[18] = 0; - out_8071961880976414619[19] = 0; - out_8071961880976414619[20] = 0; - out_8071961880976414619[21] = 0; - out_8071961880976414619[22] = 0; - out_8071961880976414619[23] = 0; - out_8071961880976414619[24] = 1; - out_8071961880976414619[25] = 0; - out_8071961880976414619[26] = 0; - out_8071961880976414619[27] = dt; - out_8071961880976414619[28] = 0; - out_8071961880976414619[29] = 0; - out_8071961880976414619[30] = 0; - out_8071961880976414619[31] = 0; - out_8071961880976414619[32] = 0; - out_8071961880976414619[33] = 0; - out_8071961880976414619[34] = 0; - out_8071961880976414619[35] = 0; - out_8071961880976414619[36] = 1; - out_8071961880976414619[37] = 0; - out_8071961880976414619[38] = 0; - out_8071961880976414619[39] = 0; - out_8071961880976414619[40] = 0; - out_8071961880976414619[41] = 0; - out_8071961880976414619[42] = 0; - out_8071961880976414619[43] = 0; - out_8071961880976414619[44] = 0; - out_8071961880976414619[45] = 0; - out_8071961880976414619[46] = 0; - out_8071961880976414619[47] = 0; - out_8071961880976414619[48] = 1; - out_8071961880976414619[49] = 0; - out_8071961880976414619[50] = 0; - out_8071961880976414619[51] = 0; - out_8071961880976414619[52] = 0; - out_8071961880976414619[53] = 0; - out_8071961880976414619[54] = 0; - out_8071961880976414619[55] = 0; - out_8071961880976414619[56] = 0; - out_8071961880976414619[57] = 0; - out_8071961880976414619[58] = 0; - out_8071961880976414619[59] = 0; - out_8071961880976414619[60] = 1; - out_8071961880976414619[61] = 0; - out_8071961880976414619[62] = 0; - out_8071961880976414619[63] = 0; - out_8071961880976414619[64] = 0; - out_8071961880976414619[65] = 0; - out_8071961880976414619[66] = 0; - out_8071961880976414619[67] = 0; - out_8071961880976414619[68] = 0; - out_8071961880976414619[69] = 0; - out_8071961880976414619[70] = 0; - out_8071961880976414619[71] = 0; - out_8071961880976414619[72] = 1; - out_8071961880976414619[73] = dt; - out_8071961880976414619[74] = 0; - out_8071961880976414619[75] = 0; - out_8071961880976414619[76] = 0; - out_8071961880976414619[77] = 0; - out_8071961880976414619[78] = 0; - out_8071961880976414619[79] = 0; - out_8071961880976414619[80] = 0; - out_8071961880976414619[81] = 0; - out_8071961880976414619[82] = 0; - out_8071961880976414619[83] = 0; - out_8071961880976414619[84] = 1; - out_8071961880976414619[85] = dt; - out_8071961880976414619[86] = 0; - out_8071961880976414619[87] = 0; - out_8071961880976414619[88] = 0; - out_8071961880976414619[89] = 0; - out_8071961880976414619[90] = 0; - out_8071961880976414619[91] = 0; - out_8071961880976414619[92] = 0; - out_8071961880976414619[93] = 0; - out_8071961880976414619[94] = 0; - out_8071961880976414619[95] = 0; - out_8071961880976414619[96] = 1; - out_8071961880976414619[97] = 0; - out_8071961880976414619[98] = 0; - out_8071961880976414619[99] = 0; - out_8071961880976414619[100] = 0; - out_8071961880976414619[101] = 0; - out_8071961880976414619[102] = 0; - out_8071961880976414619[103] = 0; - out_8071961880976414619[104] = 0; - out_8071961880976414619[105] = 0; - out_8071961880976414619[106] = 0; - out_8071961880976414619[107] = 0; - out_8071961880976414619[108] = 1; - out_8071961880976414619[109] = 0; - out_8071961880976414619[110] = 0; - out_8071961880976414619[111] = 0; - out_8071961880976414619[112] = 0; - out_8071961880976414619[113] = 0; - out_8071961880976414619[114] = 0; - out_8071961880976414619[115] = 0; - out_8071961880976414619[116] = 0; - out_8071961880976414619[117] = 0; - out_8071961880976414619[118] = 0; - out_8071961880976414619[119] = 0; - out_8071961880976414619[120] = 1; +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 h_6(double *state, double *sat_pos, double *out_3924727623087114070) { - out_3924727623087114070[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_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_3168437365824362595) { - out_3168437365824362595[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_3168437365824362595[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_3168437365824362595[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_3168437365824362595[3] = 0; - out_3168437365824362595[4] = 0; - out_3168437365824362595[5] = 0; - out_3168437365824362595[6] = 1; - out_3168437365824362595[7] = 0; - out_3168437365824362595[8] = 0; - out_3168437365824362595[9] = 0; - out_3168437365824362595[10] = 0; +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_20(double *state, double *sat_pos, double *out_7434817830173438872) { - out_7434817830173438872[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_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_7560749815445785198) { - out_7560749815445785198[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_7560749815445785198[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_7560749815445785198[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_7560749815445785198[3] = 0; - out_7560749815445785198[4] = 0; - out_7560749815445785198[5] = 0; - out_7560749815445785198[6] = 1; - out_7560749815445785198[7] = 0; - out_7560749815445785198[8] = 0; - out_7560749815445785198[9] = 1; - out_7560749815445785198[10] = sat_pos[3]; +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_7(double *state, double *sat_pos_vel, double *out_7603906676528442007) { - out_7603906676528442007[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_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_8038884862828261433) { - out_8038884862828261433[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_8038884862828261433[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_8038884862828261433[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_8038884862828261433[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_8038884862828261433[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_8038884862828261433[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_8038884862828261433[6] = 0; - out_8038884862828261433[7] = 1; - out_8038884862828261433[8] = 0; - out_8038884862828261433[9] = 0; - out_8038884862828261433[10] = 0; +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_21(double *state, double *sat_pos_vel, double *out_7603906676528442007) { - out_7603906676528442007[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_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_8038884862828261433) { - out_8038884862828261433[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_8038884862828261433[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_8038884862828261433[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_8038884862828261433[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_8038884862828261433[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_8038884862828261433[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_8038884862828261433[6] = 0; - out_8038884862828261433[7] = 1; - out_8038884862828261433[8] = 0; - out_8038884862828261433[9] = 0; - out_8038884862828261433[10] = 0; +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; } #include #include @@ -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_6740791193231733360) { - err_fun(nom_x, delta_x, out_6740791193231733360); +void gnss_err_fun(double *nom_x, double *delta_x, double *out_2351947283659598660) { + err_fun(nom_x, delta_x, out_2351947283659598660); } -void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_5435998030533772419) { - inv_err_fun(nom_x, true_x, out_5435998030533772419); +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_H_mod_fun(double *state, double *out_8750904480143678209) { - H_mod_fun(state, out_8750904480143678209); +void gnss_H_mod_fun(double *state, double *out_2843304307356815987) { + H_mod_fun(state, out_2843304307356815987); } -void gnss_f_fun(double *state, double dt, double *out_571033032076185663) { - f_fun(state, dt, out_571033032076185663); +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_8071961880976414619) { - F_fun(state, dt, out_8071961880976414619); +void gnss_F_fun(double *state, double dt, double *out_4467768114936024642) { + F_fun(state, dt, out_4467768114936024642); } -void gnss_h_6(double *state, double *sat_pos, double *out_3924727623087114070) { - h_6(state, sat_pos, out_3924727623087114070); +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_3168437365824362595) { - H_6(state, sat_pos, out_3168437365824362595); +void gnss_H_6(double *state, double *sat_pos, double *out_2748205046550560630) { + H_6(state, sat_pos, out_2748205046550560630); } -void gnss_h_20(double *state, double *sat_pos, double *out_7434817830173438872) { - h_20(state, sat_pos, out_7434817830173438872); +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_7560749815445785198) { - H_20(state, sat_pos, out_7560749815445785198); +void gnss_H_20(double *state, double *sat_pos, double *out_8303647801952025828) { + H_20(state, sat_pos, out_8303647801952025828); } -void gnss_h_7(double *state, double *sat_pos_vel, double *out_7603906676528442007) { - h_7(state, sat_pos_vel, out_7603906676528442007); +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_8038884862828261433) { - H_7(state, sat_pos_vel, out_8038884862828261433); +void gnss_H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403) { + H_7(state, sat_pos_vel, out_1168235388167943403); } -void gnss_h_21(double *state, double *sat_pos_vel, double *out_7603906676528442007) { - h_21(state, sat_pos_vel, out_7603906676528442007); +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_8038884862828261433) { - H_21(state, sat_pos_vel, out_8038884862828261433); +void gnss_H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403) { + H_21(state, sat_pos_vel, out_1168235388167943403); } void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/gnss.h b/selfdrive/locationd/models/generated/gnss.h index 907739b4d..54d03e2a3 100644 --- a/selfdrive/locationd/models/generated/gnss.h +++ b/selfdrive/locationd/models/generated/gnss.h @@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void gnss_err_fun(double *nom_x, double *delta_x, double *out_6740791193231733360); -void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_5435998030533772419); -void gnss_H_mod_fun(double *state, double *out_8750904480143678209); -void gnss_f_fun(double *state, double dt, double *out_571033032076185663); -void gnss_F_fun(double *state, double dt, double *out_8071961880976414619); -void gnss_h_6(double *state, double *sat_pos, double *out_3924727623087114070); -void gnss_H_6(double *state, double *sat_pos, double *out_3168437365824362595); -void gnss_h_20(double *state, double *sat_pos, double *out_7434817830173438872); -void gnss_H_20(double *state, double *sat_pos, double *out_7560749815445785198); -void gnss_h_7(double *state, double *sat_pos_vel, double *out_7603906676528442007); -void gnss_H_7(double *state, double *sat_pos_vel, double *out_8038884862828261433); -void gnss_h_21(double *state, double *sat_pos_vel, double *out_7603906676528442007); -void gnss_H_21(double *state, double *sat_pos_vel, double *out_8038884862828261433); +void gnss_err_fun(double *nom_x, double *delta_x, double *out_2351947283659598660); +void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653); +void gnss_H_mod_fun(double *state, double *out_2843304307356815987); +void gnss_f_fun(double *state, double dt, double *out_774769938980785647); +void gnss_F_fun(double *state, double dt, double *out_4467768114936024642); +void gnss_h_6(double *state, double *sat_pos, double *out_8206867657584711155); +void gnss_H_6(double *state, double *sat_pos, double *out_2748205046550560630); +void gnss_h_20(double *state, double *sat_pos, double *out_3144072620339266513); +void gnss_H_20(double *state, double *sat_pos, double *out_8303647801952025828); +void gnss_h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339); +void gnss_H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403); +void gnss_h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339); +void gnss_H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403); void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/libkf.so b/selfdrive/locationd/models/generated/libkf.so index 73e673297..a872de715 100755 Binary files a/selfdrive/locationd/models/generated/libkf.so and b/selfdrive/locationd/models/generated/libkf.so differ diff --git a/selfdrive/locationd/models/generated/live.cpp b/selfdrive/locationd/models/generated/live.cpp index ced097a81..0c0281f27 100644 --- a/selfdrive/locationd/models/generated/live.cpp +++ b/selfdrive/locationd/models/generated/live.cpp @@ -22,1683 +22,1683 @@ const static double MAHA_THRESH_33 = 7.814727903251177; * * * This file is part of 'ekf' * ******************************************************************************/ -void H(double *in_vec, double *out_3693999405139314794) { - out_3693999405139314794[0] = 0; - out_3693999405139314794[1] = -sin(in_vec[1])*sin(in_vec[2])*in_vec[4] - sin(in_vec[1])*cos(in_vec[2])*in_vec[3] - cos(in_vec[1])*in_vec[5]; - out_3693999405139314794[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4]; - out_3693999405139314794[3] = cos(in_vec[1])*cos(in_vec[2]); - out_3693999405139314794[4] = sin(in_vec[2])*cos(in_vec[1]); - out_3693999405139314794[5] = -sin(in_vec[1]); - out_3693999405139314794[6] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (-sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*in_vec[5]; - out_3693999405139314794[7] = -sin(in_vec[0])*sin(in_vec[1])*in_vec[5] + sin(in_vec[0])*sin(in_vec[2])*cos(in_vec[1])*in_vec[4] + sin(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; - out_3693999405139314794[8] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]))*in_vec[4]; - out_3693999405139314794[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]); - out_3693999405139314794[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]); - out_3693999405139314794[11] = sin(in_vec[0])*cos(in_vec[1]); - out_3693999405139314794[12] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (-sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) + sin(in_vec[2])*cos(in_vec[0]))*in_vec[3] - sin(in_vec[0])*cos(in_vec[1])*in_vec[5]; - out_3693999405139314794[13] = -sin(in_vec[1])*cos(in_vec[0])*in_vec[5] + sin(in_vec[2])*cos(in_vec[0])*cos(in_vec[1])*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; - out_3693999405139314794[14] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (sin(in_vec[0])*cos(in_vec[2]) - sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[3]; - out_3693999405139314794[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]); - out_3693999405139314794[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]); - out_3693999405139314794[17] = cos(in_vec[0])*cos(in_vec[1]); +void H(double *in_vec, double *out_8687680363762493539) { + out_8687680363762493539[0] = 0; + out_8687680363762493539[1] = -sin(in_vec[1])*sin(in_vec[2])*in_vec[4] - sin(in_vec[1])*cos(in_vec[2])*in_vec[3] - cos(in_vec[1])*in_vec[5]; + out_8687680363762493539[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4]; + out_8687680363762493539[3] = cos(in_vec[1])*cos(in_vec[2]); + out_8687680363762493539[4] = sin(in_vec[2])*cos(in_vec[1]); + out_8687680363762493539[5] = -sin(in_vec[1]); + out_8687680363762493539[6] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (-sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*in_vec[5]; + out_8687680363762493539[7] = -sin(in_vec[0])*sin(in_vec[1])*in_vec[5] + sin(in_vec[0])*sin(in_vec[2])*cos(in_vec[1])*in_vec[4] + sin(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; + out_8687680363762493539[8] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]))*in_vec[4]; + out_8687680363762493539[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]); + out_8687680363762493539[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]); + out_8687680363762493539[11] = sin(in_vec[0])*cos(in_vec[1]); + out_8687680363762493539[12] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (-sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) + sin(in_vec[2])*cos(in_vec[0]))*in_vec[3] - sin(in_vec[0])*cos(in_vec[1])*in_vec[5]; + out_8687680363762493539[13] = -sin(in_vec[1])*cos(in_vec[0])*in_vec[5] + sin(in_vec[2])*cos(in_vec[0])*cos(in_vec[1])*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; + out_8687680363762493539[14] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (sin(in_vec[0])*cos(in_vec[2]) - sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[3]; + out_8687680363762493539[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]); + out_8687680363762493539[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]); + out_8687680363762493539[17] = cos(in_vec[0])*cos(in_vec[1]); } -void err_fun(double *nom_x, double *delta_x, double *out_3414598820419176679) { - out_3414598820419176679[0] = delta_x[0] + nom_x[0]; - out_3414598820419176679[1] = delta_x[1] + nom_x[1]; - out_3414598820419176679[2] = delta_x[2] + nom_x[2]; - out_3414598820419176679[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3]; - out_3414598820419176679[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4]; - out_3414598820419176679[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5]; - out_3414598820419176679[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6]; - out_3414598820419176679[7] = delta_x[6] + nom_x[7]; - out_3414598820419176679[8] = delta_x[7] + nom_x[8]; - out_3414598820419176679[9] = delta_x[8] + nom_x[9]; - out_3414598820419176679[10] = delta_x[9] + nom_x[10]; - out_3414598820419176679[11] = delta_x[10] + nom_x[11]; - out_3414598820419176679[12] = delta_x[11] + nom_x[12]; - out_3414598820419176679[13] = delta_x[12] + nom_x[13]; - out_3414598820419176679[14] = delta_x[13] + nom_x[14]; - out_3414598820419176679[15] = delta_x[14] + nom_x[15]; - out_3414598820419176679[16] = delta_x[15] + nom_x[16]; - out_3414598820419176679[17] = delta_x[16] + nom_x[17]; - out_3414598820419176679[18] = delta_x[17] + nom_x[18]; - out_3414598820419176679[19] = delta_x[18] + nom_x[19]; - out_3414598820419176679[20] = delta_x[19] + nom_x[20]; - out_3414598820419176679[21] = delta_x[20] + nom_x[21]; +void err_fun(double *nom_x, double *delta_x, double *out_8136490426412747881) { + out_8136490426412747881[0] = delta_x[0] + nom_x[0]; + out_8136490426412747881[1] = delta_x[1] + nom_x[1]; + out_8136490426412747881[2] = delta_x[2] + nom_x[2]; + out_8136490426412747881[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3]; + out_8136490426412747881[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4]; + out_8136490426412747881[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5]; + out_8136490426412747881[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6]; + out_8136490426412747881[7] = delta_x[6] + nom_x[7]; + out_8136490426412747881[8] = delta_x[7] + nom_x[8]; + out_8136490426412747881[9] = delta_x[8] + nom_x[9]; + out_8136490426412747881[10] = delta_x[9] + nom_x[10]; + out_8136490426412747881[11] = delta_x[10] + nom_x[11]; + out_8136490426412747881[12] = delta_x[11] + nom_x[12]; + out_8136490426412747881[13] = delta_x[12] + nom_x[13]; + out_8136490426412747881[14] = delta_x[13] + nom_x[14]; + out_8136490426412747881[15] = delta_x[14] + nom_x[15]; + out_8136490426412747881[16] = delta_x[15] + nom_x[16]; + out_8136490426412747881[17] = delta_x[16] + nom_x[17]; + out_8136490426412747881[18] = delta_x[17] + nom_x[18]; + out_8136490426412747881[19] = delta_x[18] + nom_x[19]; + out_8136490426412747881[20] = delta_x[19] + nom_x[20]; + out_8136490426412747881[21] = delta_x[20] + nom_x[21]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5364111469669926070) { - out_5364111469669926070[0] = -nom_x[0] + true_x[0]; - out_5364111469669926070[1] = -nom_x[1] + true_x[1]; - out_5364111469669926070[2] = -nom_x[2] + true_x[2]; - out_5364111469669926070[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5]; - out_5364111469669926070[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4]; - out_5364111469669926070[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3]; - out_5364111469669926070[6] = -nom_x[7] + true_x[7]; - out_5364111469669926070[7] = -nom_x[8] + true_x[8]; - out_5364111469669926070[8] = -nom_x[9] + true_x[9]; - out_5364111469669926070[9] = -nom_x[10] + true_x[10]; - out_5364111469669926070[10] = -nom_x[11] + true_x[11]; - out_5364111469669926070[11] = -nom_x[12] + true_x[12]; - out_5364111469669926070[12] = -nom_x[13] + true_x[13]; - out_5364111469669926070[13] = -nom_x[14] + true_x[14]; - out_5364111469669926070[14] = -nom_x[15] + true_x[15]; - out_5364111469669926070[15] = -nom_x[16] + true_x[16]; - out_5364111469669926070[16] = -nom_x[17] + true_x[17]; - out_5364111469669926070[17] = -nom_x[18] + true_x[18]; - out_5364111469669926070[18] = -nom_x[19] + true_x[19]; - out_5364111469669926070[19] = -nom_x[20] + true_x[20]; - out_5364111469669926070[20] = -nom_x[21] + true_x[21]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1524655933590429885) { + out_1524655933590429885[0] = -nom_x[0] + true_x[0]; + out_1524655933590429885[1] = -nom_x[1] + true_x[1]; + out_1524655933590429885[2] = -nom_x[2] + true_x[2]; + out_1524655933590429885[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5]; + out_1524655933590429885[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4]; + out_1524655933590429885[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3]; + out_1524655933590429885[6] = -nom_x[7] + true_x[7]; + out_1524655933590429885[7] = -nom_x[8] + true_x[8]; + out_1524655933590429885[8] = -nom_x[9] + true_x[9]; + out_1524655933590429885[9] = -nom_x[10] + true_x[10]; + out_1524655933590429885[10] = -nom_x[11] + true_x[11]; + out_1524655933590429885[11] = -nom_x[12] + true_x[12]; + out_1524655933590429885[12] = -nom_x[13] + true_x[13]; + out_1524655933590429885[13] = -nom_x[14] + true_x[14]; + out_1524655933590429885[14] = -nom_x[15] + true_x[15]; + out_1524655933590429885[15] = -nom_x[16] + true_x[16]; + out_1524655933590429885[16] = -nom_x[17] + true_x[17]; + out_1524655933590429885[17] = -nom_x[18] + true_x[18]; + out_1524655933590429885[18] = -nom_x[19] + true_x[19]; + out_1524655933590429885[19] = -nom_x[20] + true_x[20]; + out_1524655933590429885[20] = -nom_x[21] + true_x[21]; } -void H_mod_fun(double *state, double *out_8664355600254952032) { - out_8664355600254952032[0] = 1.0; - out_8664355600254952032[1] = 0; - out_8664355600254952032[2] = 0; - out_8664355600254952032[3] = 0; - out_8664355600254952032[4] = 0; - out_8664355600254952032[5] = 0; - out_8664355600254952032[6] = 0; - out_8664355600254952032[7] = 0; - out_8664355600254952032[8] = 0; - out_8664355600254952032[9] = 0; - out_8664355600254952032[10] = 0; - out_8664355600254952032[11] = 0; - out_8664355600254952032[12] = 0; - out_8664355600254952032[13] = 0; - out_8664355600254952032[14] = 0; - out_8664355600254952032[15] = 0; - out_8664355600254952032[16] = 0; - out_8664355600254952032[17] = 0; - out_8664355600254952032[18] = 0; - out_8664355600254952032[19] = 0; - out_8664355600254952032[20] = 0; - out_8664355600254952032[21] = 0; - out_8664355600254952032[22] = 1.0; - out_8664355600254952032[23] = 0; - out_8664355600254952032[24] = 0; - out_8664355600254952032[25] = 0; - out_8664355600254952032[26] = 0; - out_8664355600254952032[27] = 0; - out_8664355600254952032[28] = 0; - out_8664355600254952032[29] = 0; - out_8664355600254952032[30] = 0; - out_8664355600254952032[31] = 0; - out_8664355600254952032[32] = 0; - out_8664355600254952032[33] = 0; - out_8664355600254952032[34] = 0; - out_8664355600254952032[35] = 0; - out_8664355600254952032[36] = 0; - out_8664355600254952032[37] = 0; - out_8664355600254952032[38] = 0; - out_8664355600254952032[39] = 0; - out_8664355600254952032[40] = 0; - out_8664355600254952032[41] = 0; - out_8664355600254952032[42] = 0; - out_8664355600254952032[43] = 0; - out_8664355600254952032[44] = 1.0; - out_8664355600254952032[45] = 0; - out_8664355600254952032[46] = 0; - out_8664355600254952032[47] = 0; - out_8664355600254952032[48] = 0; - out_8664355600254952032[49] = 0; - out_8664355600254952032[50] = 0; - out_8664355600254952032[51] = 0; - out_8664355600254952032[52] = 0; - out_8664355600254952032[53] = 0; - out_8664355600254952032[54] = 0; - out_8664355600254952032[55] = 0; - out_8664355600254952032[56] = 0; - out_8664355600254952032[57] = 0; - out_8664355600254952032[58] = 0; - out_8664355600254952032[59] = 0; - out_8664355600254952032[60] = 0; - out_8664355600254952032[61] = 0; - out_8664355600254952032[62] = 0; - out_8664355600254952032[63] = 0; - out_8664355600254952032[64] = 0; - out_8664355600254952032[65] = 0; - out_8664355600254952032[66] = -0.5*state[4]; - out_8664355600254952032[67] = -0.5*state[5]; - out_8664355600254952032[68] = -0.5*state[6]; - out_8664355600254952032[69] = 0; - out_8664355600254952032[70] = 0; - out_8664355600254952032[71] = 0; - out_8664355600254952032[72] = 0; - out_8664355600254952032[73] = 0; - out_8664355600254952032[74] = 0; - out_8664355600254952032[75] = 0; - out_8664355600254952032[76] = 0; - out_8664355600254952032[77] = 0; - out_8664355600254952032[78] = 0; - out_8664355600254952032[79] = 0; - out_8664355600254952032[80] = 0; - out_8664355600254952032[81] = 0; - out_8664355600254952032[82] = 0; - out_8664355600254952032[83] = 0; - out_8664355600254952032[84] = 0; - out_8664355600254952032[85] = 0; - out_8664355600254952032[86] = 0; - out_8664355600254952032[87] = 0.5*state[3]; - out_8664355600254952032[88] = 0.5*state[6]; - out_8664355600254952032[89] = -0.5*state[5]; - out_8664355600254952032[90] = 0; - out_8664355600254952032[91] = 0; - out_8664355600254952032[92] = 0; - out_8664355600254952032[93] = 0; - out_8664355600254952032[94] = 0; - out_8664355600254952032[95] = 0; - out_8664355600254952032[96] = 0; - out_8664355600254952032[97] = 0; - out_8664355600254952032[98] = 0; - out_8664355600254952032[99] = 0; - out_8664355600254952032[100] = 0; - out_8664355600254952032[101] = 0; - out_8664355600254952032[102] = 0; - out_8664355600254952032[103] = 0; - out_8664355600254952032[104] = 0; - out_8664355600254952032[105] = 0; - out_8664355600254952032[106] = 0; - out_8664355600254952032[107] = 0; - out_8664355600254952032[108] = -0.5*state[6]; - out_8664355600254952032[109] = 0.5*state[3]; - out_8664355600254952032[110] = 0.5*state[4]; - out_8664355600254952032[111] = 0; - out_8664355600254952032[112] = 0; - out_8664355600254952032[113] = 0; - out_8664355600254952032[114] = 0; - out_8664355600254952032[115] = 0; - out_8664355600254952032[116] = 0; - out_8664355600254952032[117] = 0; - out_8664355600254952032[118] = 0; - out_8664355600254952032[119] = 0; - out_8664355600254952032[120] = 0; - out_8664355600254952032[121] = 0; - out_8664355600254952032[122] = 0; - out_8664355600254952032[123] = 0; - out_8664355600254952032[124] = 0; - out_8664355600254952032[125] = 0; - out_8664355600254952032[126] = 0; - out_8664355600254952032[127] = 0; - out_8664355600254952032[128] = 0; - out_8664355600254952032[129] = 0.5*state[5]; - out_8664355600254952032[130] = -0.5*state[4]; - out_8664355600254952032[131] = 0.5*state[3]; - out_8664355600254952032[132] = 0; - out_8664355600254952032[133] = 0; - out_8664355600254952032[134] = 0; - out_8664355600254952032[135] = 0; - out_8664355600254952032[136] = 0; - out_8664355600254952032[137] = 0; - out_8664355600254952032[138] = 0; - out_8664355600254952032[139] = 0; - out_8664355600254952032[140] = 0; - out_8664355600254952032[141] = 0; - out_8664355600254952032[142] = 0; - out_8664355600254952032[143] = 0; - out_8664355600254952032[144] = 0; - out_8664355600254952032[145] = 0; - out_8664355600254952032[146] = 0; - out_8664355600254952032[147] = 0; - out_8664355600254952032[148] = 0; - out_8664355600254952032[149] = 0; - out_8664355600254952032[150] = 0; - out_8664355600254952032[151] = 0; - out_8664355600254952032[152] = 0; - out_8664355600254952032[153] = 1.0; - out_8664355600254952032[154] = 0; - out_8664355600254952032[155] = 0; - out_8664355600254952032[156] = 0; - out_8664355600254952032[157] = 0; - out_8664355600254952032[158] = 0; - out_8664355600254952032[159] = 0; - out_8664355600254952032[160] = 0; - out_8664355600254952032[161] = 0; - out_8664355600254952032[162] = 0; - out_8664355600254952032[163] = 0; - out_8664355600254952032[164] = 0; - out_8664355600254952032[165] = 0; - out_8664355600254952032[166] = 0; - out_8664355600254952032[167] = 0; - out_8664355600254952032[168] = 0; - out_8664355600254952032[169] = 0; - out_8664355600254952032[170] = 0; - out_8664355600254952032[171] = 0; - out_8664355600254952032[172] = 0; - out_8664355600254952032[173] = 0; - out_8664355600254952032[174] = 0; - out_8664355600254952032[175] = 1.0; - out_8664355600254952032[176] = 0; - out_8664355600254952032[177] = 0; - out_8664355600254952032[178] = 0; - out_8664355600254952032[179] = 0; - out_8664355600254952032[180] = 0; - out_8664355600254952032[181] = 0; - out_8664355600254952032[182] = 0; - out_8664355600254952032[183] = 0; - out_8664355600254952032[184] = 0; - out_8664355600254952032[185] = 0; - out_8664355600254952032[186] = 0; - out_8664355600254952032[187] = 0; - out_8664355600254952032[188] = 0; - out_8664355600254952032[189] = 0; - out_8664355600254952032[190] = 0; - out_8664355600254952032[191] = 0; - out_8664355600254952032[192] = 0; - out_8664355600254952032[193] = 0; - out_8664355600254952032[194] = 0; - out_8664355600254952032[195] = 0; - out_8664355600254952032[196] = 0; - out_8664355600254952032[197] = 1.0; - out_8664355600254952032[198] = 0; - out_8664355600254952032[199] = 0; - out_8664355600254952032[200] = 0; - out_8664355600254952032[201] = 0; - out_8664355600254952032[202] = 0; - out_8664355600254952032[203] = 0; - out_8664355600254952032[204] = 0; - out_8664355600254952032[205] = 0; - out_8664355600254952032[206] = 0; - out_8664355600254952032[207] = 0; - out_8664355600254952032[208] = 0; - out_8664355600254952032[209] = 0; - out_8664355600254952032[210] = 0; - out_8664355600254952032[211] = 0; - out_8664355600254952032[212] = 0; - out_8664355600254952032[213] = 0; - out_8664355600254952032[214] = 0; - out_8664355600254952032[215] = 0; - out_8664355600254952032[216] = 0; - out_8664355600254952032[217] = 0; - out_8664355600254952032[218] = 0; - out_8664355600254952032[219] = 1.0; - out_8664355600254952032[220] = 0; - out_8664355600254952032[221] = 0; - out_8664355600254952032[222] = 0; - out_8664355600254952032[223] = 0; - out_8664355600254952032[224] = 0; - out_8664355600254952032[225] = 0; - out_8664355600254952032[226] = 0; - out_8664355600254952032[227] = 0; - out_8664355600254952032[228] = 0; - out_8664355600254952032[229] = 0; - out_8664355600254952032[230] = 0; - out_8664355600254952032[231] = 0; - out_8664355600254952032[232] = 0; - out_8664355600254952032[233] = 0; - out_8664355600254952032[234] = 0; - out_8664355600254952032[235] = 0; - out_8664355600254952032[236] = 0; - out_8664355600254952032[237] = 0; - out_8664355600254952032[238] = 0; - out_8664355600254952032[239] = 0; - out_8664355600254952032[240] = 0; - out_8664355600254952032[241] = 1.0; - out_8664355600254952032[242] = 0; - out_8664355600254952032[243] = 0; - out_8664355600254952032[244] = 0; - out_8664355600254952032[245] = 0; - out_8664355600254952032[246] = 0; - out_8664355600254952032[247] = 0; - out_8664355600254952032[248] = 0; - out_8664355600254952032[249] = 0; - out_8664355600254952032[250] = 0; - out_8664355600254952032[251] = 0; - out_8664355600254952032[252] = 0; - out_8664355600254952032[253] = 0; - out_8664355600254952032[254] = 0; - out_8664355600254952032[255] = 0; - out_8664355600254952032[256] = 0; - out_8664355600254952032[257] = 0; - out_8664355600254952032[258] = 0; - out_8664355600254952032[259] = 0; - out_8664355600254952032[260] = 0; - out_8664355600254952032[261] = 0; - out_8664355600254952032[262] = 0; - out_8664355600254952032[263] = 1.0; - out_8664355600254952032[264] = 0; - out_8664355600254952032[265] = 0; - out_8664355600254952032[266] = 0; - out_8664355600254952032[267] = 0; - out_8664355600254952032[268] = 0; - out_8664355600254952032[269] = 0; - out_8664355600254952032[270] = 0; - out_8664355600254952032[271] = 0; - out_8664355600254952032[272] = 0; - out_8664355600254952032[273] = 0; - out_8664355600254952032[274] = 0; - out_8664355600254952032[275] = 0; - out_8664355600254952032[276] = 0; - out_8664355600254952032[277] = 0; - out_8664355600254952032[278] = 0; - out_8664355600254952032[279] = 0; - out_8664355600254952032[280] = 0; - out_8664355600254952032[281] = 0; - out_8664355600254952032[282] = 0; - out_8664355600254952032[283] = 0; - out_8664355600254952032[284] = 0; - out_8664355600254952032[285] = 1.0; - out_8664355600254952032[286] = 0; - out_8664355600254952032[287] = 0; - out_8664355600254952032[288] = 0; - out_8664355600254952032[289] = 0; - out_8664355600254952032[290] = 0; - out_8664355600254952032[291] = 0; - out_8664355600254952032[292] = 0; - out_8664355600254952032[293] = 0; - out_8664355600254952032[294] = 0; - out_8664355600254952032[295] = 0; - out_8664355600254952032[296] = 0; - out_8664355600254952032[297] = 0; - out_8664355600254952032[298] = 0; - out_8664355600254952032[299] = 0; - out_8664355600254952032[300] = 0; - out_8664355600254952032[301] = 0; - out_8664355600254952032[302] = 0; - out_8664355600254952032[303] = 0; - out_8664355600254952032[304] = 0; - out_8664355600254952032[305] = 0; - out_8664355600254952032[306] = 0; - out_8664355600254952032[307] = 1.0; - out_8664355600254952032[308] = 0; - out_8664355600254952032[309] = 0; - out_8664355600254952032[310] = 0; - out_8664355600254952032[311] = 0; - out_8664355600254952032[312] = 0; - out_8664355600254952032[313] = 0; - out_8664355600254952032[314] = 0; - out_8664355600254952032[315] = 0; - out_8664355600254952032[316] = 0; - out_8664355600254952032[317] = 0; - out_8664355600254952032[318] = 0; - out_8664355600254952032[319] = 0; - out_8664355600254952032[320] = 0; - out_8664355600254952032[321] = 0; - out_8664355600254952032[322] = 0; - out_8664355600254952032[323] = 0; - out_8664355600254952032[324] = 0; - out_8664355600254952032[325] = 0; - out_8664355600254952032[326] = 0; - out_8664355600254952032[327] = 0; - out_8664355600254952032[328] = 0; - out_8664355600254952032[329] = 1.0; - out_8664355600254952032[330] = 0; - out_8664355600254952032[331] = 0; - out_8664355600254952032[332] = 0; - out_8664355600254952032[333] = 0; - out_8664355600254952032[334] = 0; - out_8664355600254952032[335] = 0; - out_8664355600254952032[336] = 0; - out_8664355600254952032[337] = 0; - out_8664355600254952032[338] = 0; - out_8664355600254952032[339] = 0; - out_8664355600254952032[340] = 0; - out_8664355600254952032[341] = 0; - out_8664355600254952032[342] = 0; - out_8664355600254952032[343] = 0; - out_8664355600254952032[344] = 0; - out_8664355600254952032[345] = 0; - out_8664355600254952032[346] = 0; - out_8664355600254952032[347] = 0; - out_8664355600254952032[348] = 0; - out_8664355600254952032[349] = 0; - out_8664355600254952032[350] = 0; - out_8664355600254952032[351] = 1.0; - out_8664355600254952032[352] = 0; - out_8664355600254952032[353] = 0; - out_8664355600254952032[354] = 0; - out_8664355600254952032[355] = 0; - out_8664355600254952032[356] = 0; - out_8664355600254952032[357] = 0; - out_8664355600254952032[358] = 0; - out_8664355600254952032[359] = 0; - out_8664355600254952032[360] = 0; - out_8664355600254952032[361] = 0; - out_8664355600254952032[362] = 0; - out_8664355600254952032[363] = 0; - out_8664355600254952032[364] = 0; - out_8664355600254952032[365] = 0; - out_8664355600254952032[366] = 0; - out_8664355600254952032[367] = 0; - out_8664355600254952032[368] = 0; - out_8664355600254952032[369] = 0; - out_8664355600254952032[370] = 0; - out_8664355600254952032[371] = 0; - out_8664355600254952032[372] = 0; - out_8664355600254952032[373] = 1.0; - out_8664355600254952032[374] = 0; - out_8664355600254952032[375] = 0; - out_8664355600254952032[376] = 0; - out_8664355600254952032[377] = 0; - out_8664355600254952032[378] = 0; - out_8664355600254952032[379] = 0; - out_8664355600254952032[380] = 0; - out_8664355600254952032[381] = 0; - out_8664355600254952032[382] = 0; - out_8664355600254952032[383] = 0; - out_8664355600254952032[384] = 0; - out_8664355600254952032[385] = 0; - out_8664355600254952032[386] = 0; - out_8664355600254952032[387] = 0; - out_8664355600254952032[388] = 0; - out_8664355600254952032[389] = 0; - out_8664355600254952032[390] = 0; - out_8664355600254952032[391] = 0; - out_8664355600254952032[392] = 0; - out_8664355600254952032[393] = 0; - out_8664355600254952032[394] = 0; - out_8664355600254952032[395] = 1.0; - out_8664355600254952032[396] = 0; - out_8664355600254952032[397] = 0; - out_8664355600254952032[398] = 0; - out_8664355600254952032[399] = 0; - out_8664355600254952032[400] = 0; - out_8664355600254952032[401] = 0; - out_8664355600254952032[402] = 0; - out_8664355600254952032[403] = 0; - out_8664355600254952032[404] = 0; - out_8664355600254952032[405] = 0; - out_8664355600254952032[406] = 0; - out_8664355600254952032[407] = 0; - out_8664355600254952032[408] = 0; - out_8664355600254952032[409] = 0; - out_8664355600254952032[410] = 0; - out_8664355600254952032[411] = 0; - out_8664355600254952032[412] = 0; - out_8664355600254952032[413] = 0; - out_8664355600254952032[414] = 0; - out_8664355600254952032[415] = 0; - out_8664355600254952032[416] = 0; - out_8664355600254952032[417] = 1.0; - out_8664355600254952032[418] = 0; - out_8664355600254952032[419] = 0; - out_8664355600254952032[420] = 0; - out_8664355600254952032[421] = 0; - out_8664355600254952032[422] = 0; - out_8664355600254952032[423] = 0; - out_8664355600254952032[424] = 0; - out_8664355600254952032[425] = 0; - out_8664355600254952032[426] = 0; - out_8664355600254952032[427] = 0; - out_8664355600254952032[428] = 0; - out_8664355600254952032[429] = 0; - out_8664355600254952032[430] = 0; - out_8664355600254952032[431] = 0; - out_8664355600254952032[432] = 0; - out_8664355600254952032[433] = 0; - out_8664355600254952032[434] = 0; - out_8664355600254952032[435] = 0; - out_8664355600254952032[436] = 0; - out_8664355600254952032[437] = 0; - out_8664355600254952032[438] = 0; - out_8664355600254952032[439] = 1.0; - out_8664355600254952032[440] = 0; - out_8664355600254952032[441] = 0; - out_8664355600254952032[442] = 0; - out_8664355600254952032[443] = 0; - out_8664355600254952032[444] = 0; - out_8664355600254952032[445] = 0; - out_8664355600254952032[446] = 0; - out_8664355600254952032[447] = 0; - out_8664355600254952032[448] = 0; - out_8664355600254952032[449] = 0; - out_8664355600254952032[450] = 0; - out_8664355600254952032[451] = 0; - out_8664355600254952032[452] = 0; - out_8664355600254952032[453] = 0; - out_8664355600254952032[454] = 0; - out_8664355600254952032[455] = 0; - out_8664355600254952032[456] = 0; - out_8664355600254952032[457] = 0; - out_8664355600254952032[458] = 0; - out_8664355600254952032[459] = 0; - out_8664355600254952032[460] = 0; - out_8664355600254952032[461] = 1.0; +void H_mod_fun(double *state, double *out_7453067886472422609) { + out_7453067886472422609[0] = 1.0; + out_7453067886472422609[1] = 0; + out_7453067886472422609[2] = 0; + out_7453067886472422609[3] = 0; + out_7453067886472422609[4] = 0; + out_7453067886472422609[5] = 0; + out_7453067886472422609[6] = 0; + out_7453067886472422609[7] = 0; + out_7453067886472422609[8] = 0; + out_7453067886472422609[9] = 0; + out_7453067886472422609[10] = 0; + out_7453067886472422609[11] = 0; + out_7453067886472422609[12] = 0; + out_7453067886472422609[13] = 0; + out_7453067886472422609[14] = 0; + out_7453067886472422609[15] = 0; + out_7453067886472422609[16] = 0; + out_7453067886472422609[17] = 0; + out_7453067886472422609[18] = 0; + out_7453067886472422609[19] = 0; + out_7453067886472422609[20] = 0; + out_7453067886472422609[21] = 0; + out_7453067886472422609[22] = 1.0; + out_7453067886472422609[23] = 0; + out_7453067886472422609[24] = 0; + out_7453067886472422609[25] = 0; + out_7453067886472422609[26] = 0; + out_7453067886472422609[27] = 0; + out_7453067886472422609[28] = 0; + out_7453067886472422609[29] = 0; + out_7453067886472422609[30] = 0; + out_7453067886472422609[31] = 0; + out_7453067886472422609[32] = 0; + out_7453067886472422609[33] = 0; + out_7453067886472422609[34] = 0; + out_7453067886472422609[35] = 0; + out_7453067886472422609[36] = 0; + out_7453067886472422609[37] = 0; + out_7453067886472422609[38] = 0; + out_7453067886472422609[39] = 0; + out_7453067886472422609[40] = 0; + out_7453067886472422609[41] = 0; + out_7453067886472422609[42] = 0; + out_7453067886472422609[43] = 0; + out_7453067886472422609[44] = 1.0; + out_7453067886472422609[45] = 0; + out_7453067886472422609[46] = 0; + out_7453067886472422609[47] = 0; + out_7453067886472422609[48] = 0; + out_7453067886472422609[49] = 0; + out_7453067886472422609[50] = 0; + out_7453067886472422609[51] = 0; + out_7453067886472422609[52] = 0; + out_7453067886472422609[53] = 0; + out_7453067886472422609[54] = 0; + out_7453067886472422609[55] = 0; + out_7453067886472422609[56] = 0; + out_7453067886472422609[57] = 0; + out_7453067886472422609[58] = 0; + out_7453067886472422609[59] = 0; + out_7453067886472422609[60] = 0; + out_7453067886472422609[61] = 0; + out_7453067886472422609[62] = 0; + out_7453067886472422609[63] = 0; + out_7453067886472422609[64] = 0; + out_7453067886472422609[65] = 0; + out_7453067886472422609[66] = -0.5*state[4]; + out_7453067886472422609[67] = -0.5*state[5]; + out_7453067886472422609[68] = -0.5*state[6]; + out_7453067886472422609[69] = 0; + out_7453067886472422609[70] = 0; + out_7453067886472422609[71] = 0; + out_7453067886472422609[72] = 0; + out_7453067886472422609[73] = 0; + out_7453067886472422609[74] = 0; + out_7453067886472422609[75] = 0; + out_7453067886472422609[76] = 0; + out_7453067886472422609[77] = 0; + out_7453067886472422609[78] = 0; + out_7453067886472422609[79] = 0; + out_7453067886472422609[80] = 0; + out_7453067886472422609[81] = 0; + out_7453067886472422609[82] = 0; + out_7453067886472422609[83] = 0; + out_7453067886472422609[84] = 0; + out_7453067886472422609[85] = 0; + out_7453067886472422609[86] = 0; + out_7453067886472422609[87] = 0.5*state[3]; + out_7453067886472422609[88] = 0.5*state[6]; + out_7453067886472422609[89] = -0.5*state[5]; + out_7453067886472422609[90] = 0; + out_7453067886472422609[91] = 0; + out_7453067886472422609[92] = 0; + out_7453067886472422609[93] = 0; + out_7453067886472422609[94] = 0; + out_7453067886472422609[95] = 0; + out_7453067886472422609[96] = 0; + out_7453067886472422609[97] = 0; + out_7453067886472422609[98] = 0; + out_7453067886472422609[99] = 0; + out_7453067886472422609[100] = 0; + out_7453067886472422609[101] = 0; + out_7453067886472422609[102] = 0; + out_7453067886472422609[103] = 0; + out_7453067886472422609[104] = 0; + out_7453067886472422609[105] = 0; + out_7453067886472422609[106] = 0; + out_7453067886472422609[107] = 0; + out_7453067886472422609[108] = -0.5*state[6]; + out_7453067886472422609[109] = 0.5*state[3]; + out_7453067886472422609[110] = 0.5*state[4]; + out_7453067886472422609[111] = 0; + out_7453067886472422609[112] = 0; + out_7453067886472422609[113] = 0; + out_7453067886472422609[114] = 0; + out_7453067886472422609[115] = 0; + out_7453067886472422609[116] = 0; + out_7453067886472422609[117] = 0; + out_7453067886472422609[118] = 0; + out_7453067886472422609[119] = 0; + out_7453067886472422609[120] = 0; + out_7453067886472422609[121] = 0; + out_7453067886472422609[122] = 0; + out_7453067886472422609[123] = 0; + out_7453067886472422609[124] = 0; + out_7453067886472422609[125] = 0; + out_7453067886472422609[126] = 0; + out_7453067886472422609[127] = 0; + out_7453067886472422609[128] = 0; + out_7453067886472422609[129] = 0.5*state[5]; + out_7453067886472422609[130] = -0.5*state[4]; + out_7453067886472422609[131] = 0.5*state[3]; + out_7453067886472422609[132] = 0; + out_7453067886472422609[133] = 0; + out_7453067886472422609[134] = 0; + out_7453067886472422609[135] = 0; + out_7453067886472422609[136] = 0; + out_7453067886472422609[137] = 0; + out_7453067886472422609[138] = 0; + out_7453067886472422609[139] = 0; + out_7453067886472422609[140] = 0; + out_7453067886472422609[141] = 0; + out_7453067886472422609[142] = 0; + out_7453067886472422609[143] = 0; + out_7453067886472422609[144] = 0; + out_7453067886472422609[145] = 0; + out_7453067886472422609[146] = 0; + out_7453067886472422609[147] = 0; + out_7453067886472422609[148] = 0; + out_7453067886472422609[149] = 0; + out_7453067886472422609[150] = 0; + out_7453067886472422609[151] = 0; + out_7453067886472422609[152] = 0; + out_7453067886472422609[153] = 1.0; + out_7453067886472422609[154] = 0; + out_7453067886472422609[155] = 0; + out_7453067886472422609[156] = 0; + out_7453067886472422609[157] = 0; + out_7453067886472422609[158] = 0; + out_7453067886472422609[159] = 0; + out_7453067886472422609[160] = 0; + out_7453067886472422609[161] = 0; + out_7453067886472422609[162] = 0; + out_7453067886472422609[163] = 0; + out_7453067886472422609[164] = 0; + out_7453067886472422609[165] = 0; + out_7453067886472422609[166] = 0; + out_7453067886472422609[167] = 0; + out_7453067886472422609[168] = 0; + out_7453067886472422609[169] = 0; + out_7453067886472422609[170] = 0; + out_7453067886472422609[171] = 0; + out_7453067886472422609[172] = 0; + out_7453067886472422609[173] = 0; + out_7453067886472422609[174] = 0; + out_7453067886472422609[175] = 1.0; + out_7453067886472422609[176] = 0; + out_7453067886472422609[177] = 0; + out_7453067886472422609[178] = 0; + out_7453067886472422609[179] = 0; + out_7453067886472422609[180] = 0; + out_7453067886472422609[181] = 0; + out_7453067886472422609[182] = 0; + out_7453067886472422609[183] = 0; + out_7453067886472422609[184] = 0; + out_7453067886472422609[185] = 0; + out_7453067886472422609[186] = 0; + out_7453067886472422609[187] = 0; + out_7453067886472422609[188] = 0; + out_7453067886472422609[189] = 0; + out_7453067886472422609[190] = 0; + out_7453067886472422609[191] = 0; + out_7453067886472422609[192] = 0; + out_7453067886472422609[193] = 0; + out_7453067886472422609[194] = 0; + out_7453067886472422609[195] = 0; + out_7453067886472422609[196] = 0; + out_7453067886472422609[197] = 1.0; + out_7453067886472422609[198] = 0; + out_7453067886472422609[199] = 0; + out_7453067886472422609[200] = 0; + out_7453067886472422609[201] = 0; + out_7453067886472422609[202] = 0; + out_7453067886472422609[203] = 0; + out_7453067886472422609[204] = 0; + out_7453067886472422609[205] = 0; + out_7453067886472422609[206] = 0; + out_7453067886472422609[207] = 0; + out_7453067886472422609[208] = 0; + out_7453067886472422609[209] = 0; + out_7453067886472422609[210] = 0; + out_7453067886472422609[211] = 0; + out_7453067886472422609[212] = 0; + out_7453067886472422609[213] = 0; + out_7453067886472422609[214] = 0; + out_7453067886472422609[215] = 0; + out_7453067886472422609[216] = 0; + out_7453067886472422609[217] = 0; + out_7453067886472422609[218] = 0; + out_7453067886472422609[219] = 1.0; + out_7453067886472422609[220] = 0; + out_7453067886472422609[221] = 0; + out_7453067886472422609[222] = 0; + out_7453067886472422609[223] = 0; + out_7453067886472422609[224] = 0; + out_7453067886472422609[225] = 0; + out_7453067886472422609[226] = 0; + out_7453067886472422609[227] = 0; + out_7453067886472422609[228] = 0; + out_7453067886472422609[229] = 0; + out_7453067886472422609[230] = 0; + out_7453067886472422609[231] = 0; + out_7453067886472422609[232] = 0; + out_7453067886472422609[233] = 0; + out_7453067886472422609[234] = 0; + out_7453067886472422609[235] = 0; + out_7453067886472422609[236] = 0; + out_7453067886472422609[237] = 0; + out_7453067886472422609[238] = 0; + out_7453067886472422609[239] = 0; + out_7453067886472422609[240] = 0; + out_7453067886472422609[241] = 1.0; + out_7453067886472422609[242] = 0; + out_7453067886472422609[243] = 0; + out_7453067886472422609[244] = 0; + out_7453067886472422609[245] = 0; + out_7453067886472422609[246] = 0; + out_7453067886472422609[247] = 0; + out_7453067886472422609[248] = 0; + out_7453067886472422609[249] = 0; + out_7453067886472422609[250] = 0; + out_7453067886472422609[251] = 0; + out_7453067886472422609[252] = 0; + out_7453067886472422609[253] = 0; + out_7453067886472422609[254] = 0; + out_7453067886472422609[255] = 0; + out_7453067886472422609[256] = 0; + out_7453067886472422609[257] = 0; + out_7453067886472422609[258] = 0; + out_7453067886472422609[259] = 0; + out_7453067886472422609[260] = 0; + out_7453067886472422609[261] = 0; + out_7453067886472422609[262] = 0; + out_7453067886472422609[263] = 1.0; + out_7453067886472422609[264] = 0; + out_7453067886472422609[265] = 0; + out_7453067886472422609[266] = 0; + out_7453067886472422609[267] = 0; + out_7453067886472422609[268] = 0; + out_7453067886472422609[269] = 0; + out_7453067886472422609[270] = 0; + out_7453067886472422609[271] = 0; + out_7453067886472422609[272] = 0; + out_7453067886472422609[273] = 0; + out_7453067886472422609[274] = 0; + out_7453067886472422609[275] = 0; + out_7453067886472422609[276] = 0; + out_7453067886472422609[277] = 0; + out_7453067886472422609[278] = 0; + out_7453067886472422609[279] = 0; + out_7453067886472422609[280] = 0; + out_7453067886472422609[281] = 0; + out_7453067886472422609[282] = 0; + out_7453067886472422609[283] = 0; + out_7453067886472422609[284] = 0; + out_7453067886472422609[285] = 1.0; + out_7453067886472422609[286] = 0; + out_7453067886472422609[287] = 0; + out_7453067886472422609[288] = 0; + out_7453067886472422609[289] = 0; + out_7453067886472422609[290] = 0; + out_7453067886472422609[291] = 0; + out_7453067886472422609[292] = 0; + out_7453067886472422609[293] = 0; + out_7453067886472422609[294] = 0; + out_7453067886472422609[295] = 0; + out_7453067886472422609[296] = 0; + out_7453067886472422609[297] = 0; + out_7453067886472422609[298] = 0; + out_7453067886472422609[299] = 0; + out_7453067886472422609[300] = 0; + out_7453067886472422609[301] = 0; + out_7453067886472422609[302] = 0; + out_7453067886472422609[303] = 0; + out_7453067886472422609[304] = 0; + out_7453067886472422609[305] = 0; + out_7453067886472422609[306] = 0; + out_7453067886472422609[307] = 1.0; + out_7453067886472422609[308] = 0; + out_7453067886472422609[309] = 0; + out_7453067886472422609[310] = 0; + out_7453067886472422609[311] = 0; + out_7453067886472422609[312] = 0; + out_7453067886472422609[313] = 0; + out_7453067886472422609[314] = 0; + out_7453067886472422609[315] = 0; + out_7453067886472422609[316] = 0; + out_7453067886472422609[317] = 0; + out_7453067886472422609[318] = 0; + out_7453067886472422609[319] = 0; + out_7453067886472422609[320] = 0; + out_7453067886472422609[321] = 0; + out_7453067886472422609[322] = 0; + out_7453067886472422609[323] = 0; + out_7453067886472422609[324] = 0; + out_7453067886472422609[325] = 0; + out_7453067886472422609[326] = 0; + out_7453067886472422609[327] = 0; + out_7453067886472422609[328] = 0; + out_7453067886472422609[329] = 1.0; + out_7453067886472422609[330] = 0; + out_7453067886472422609[331] = 0; + out_7453067886472422609[332] = 0; + out_7453067886472422609[333] = 0; + out_7453067886472422609[334] = 0; + out_7453067886472422609[335] = 0; + out_7453067886472422609[336] = 0; + out_7453067886472422609[337] = 0; + out_7453067886472422609[338] = 0; + out_7453067886472422609[339] = 0; + out_7453067886472422609[340] = 0; + out_7453067886472422609[341] = 0; + out_7453067886472422609[342] = 0; + out_7453067886472422609[343] = 0; + out_7453067886472422609[344] = 0; + out_7453067886472422609[345] = 0; + out_7453067886472422609[346] = 0; + out_7453067886472422609[347] = 0; + out_7453067886472422609[348] = 0; + out_7453067886472422609[349] = 0; + out_7453067886472422609[350] = 0; + out_7453067886472422609[351] = 1.0; + out_7453067886472422609[352] = 0; + out_7453067886472422609[353] = 0; + out_7453067886472422609[354] = 0; + out_7453067886472422609[355] = 0; + out_7453067886472422609[356] = 0; + out_7453067886472422609[357] = 0; + out_7453067886472422609[358] = 0; + out_7453067886472422609[359] = 0; + out_7453067886472422609[360] = 0; + out_7453067886472422609[361] = 0; + out_7453067886472422609[362] = 0; + out_7453067886472422609[363] = 0; + out_7453067886472422609[364] = 0; + out_7453067886472422609[365] = 0; + out_7453067886472422609[366] = 0; + out_7453067886472422609[367] = 0; + out_7453067886472422609[368] = 0; + out_7453067886472422609[369] = 0; + out_7453067886472422609[370] = 0; + out_7453067886472422609[371] = 0; + out_7453067886472422609[372] = 0; + out_7453067886472422609[373] = 1.0; + out_7453067886472422609[374] = 0; + out_7453067886472422609[375] = 0; + out_7453067886472422609[376] = 0; + out_7453067886472422609[377] = 0; + out_7453067886472422609[378] = 0; + out_7453067886472422609[379] = 0; + out_7453067886472422609[380] = 0; + out_7453067886472422609[381] = 0; + out_7453067886472422609[382] = 0; + out_7453067886472422609[383] = 0; + out_7453067886472422609[384] = 0; + out_7453067886472422609[385] = 0; + out_7453067886472422609[386] = 0; + out_7453067886472422609[387] = 0; + out_7453067886472422609[388] = 0; + out_7453067886472422609[389] = 0; + out_7453067886472422609[390] = 0; + out_7453067886472422609[391] = 0; + out_7453067886472422609[392] = 0; + out_7453067886472422609[393] = 0; + out_7453067886472422609[394] = 0; + out_7453067886472422609[395] = 1.0; + out_7453067886472422609[396] = 0; + out_7453067886472422609[397] = 0; + out_7453067886472422609[398] = 0; + out_7453067886472422609[399] = 0; + out_7453067886472422609[400] = 0; + out_7453067886472422609[401] = 0; + out_7453067886472422609[402] = 0; + out_7453067886472422609[403] = 0; + out_7453067886472422609[404] = 0; + out_7453067886472422609[405] = 0; + out_7453067886472422609[406] = 0; + out_7453067886472422609[407] = 0; + out_7453067886472422609[408] = 0; + out_7453067886472422609[409] = 0; + out_7453067886472422609[410] = 0; + out_7453067886472422609[411] = 0; + out_7453067886472422609[412] = 0; + out_7453067886472422609[413] = 0; + out_7453067886472422609[414] = 0; + out_7453067886472422609[415] = 0; + out_7453067886472422609[416] = 0; + out_7453067886472422609[417] = 1.0; + out_7453067886472422609[418] = 0; + out_7453067886472422609[419] = 0; + out_7453067886472422609[420] = 0; + out_7453067886472422609[421] = 0; + out_7453067886472422609[422] = 0; + out_7453067886472422609[423] = 0; + out_7453067886472422609[424] = 0; + out_7453067886472422609[425] = 0; + out_7453067886472422609[426] = 0; + out_7453067886472422609[427] = 0; + out_7453067886472422609[428] = 0; + out_7453067886472422609[429] = 0; + out_7453067886472422609[430] = 0; + out_7453067886472422609[431] = 0; + out_7453067886472422609[432] = 0; + out_7453067886472422609[433] = 0; + out_7453067886472422609[434] = 0; + out_7453067886472422609[435] = 0; + out_7453067886472422609[436] = 0; + out_7453067886472422609[437] = 0; + out_7453067886472422609[438] = 0; + out_7453067886472422609[439] = 1.0; + out_7453067886472422609[440] = 0; + out_7453067886472422609[441] = 0; + out_7453067886472422609[442] = 0; + out_7453067886472422609[443] = 0; + out_7453067886472422609[444] = 0; + out_7453067886472422609[445] = 0; + out_7453067886472422609[446] = 0; + out_7453067886472422609[447] = 0; + out_7453067886472422609[448] = 0; + out_7453067886472422609[449] = 0; + out_7453067886472422609[450] = 0; + out_7453067886472422609[451] = 0; + out_7453067886472422609[452] = 0; + out_7453067886472422609[453] = 0; + out_7453067886472422609[454] = 0; + out_7453067886472422609[455] = 0; + out_7453067886472422609[456] = 0; + out_7453067886472422609[457] = 0; + out_7453067886472422609[458] = 0; + out_7453067886472422609[459] = 0; + out_7453067886472422609[460] = 0; + out_7453067886472422609[461] = 1.0; } -void f_fun(double *state, double dt, double *out_5944879704050977948) { - out_5944879704050977948[0] = dt*state[7] + state[0]; - out_5944879704050977948[1] = dt*state[8] + state[1]; - out_5944879704050977948[2] = dt*state[9] + state[2]; - out_5944879704050977948[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3]; - out_5944879704050977948[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4]; - out_5944879704050977948[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5]; - out_5944879704050977948[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6]; - out_5944879704050977948[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]) + state[7]; - out_5944879704050977948[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[8]; - out_5944879704050977948[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]) + state[9]; - out_5944879704050977948[10] = state[10]; - out_5944879704050977948[11] = state[11]; - out_5944879704050977948[12] = state[12]; - out_5944879704050977948[13] = state[13]; - out_5944879704050977948[14] = state[14]; - out_5944879704050977948[15] = state[15]; - out_5944879704050977948[16] = state[16]; - out_5944879704050977948[17] = state[17]; - out_5944879704050977948[18] = state[18]; - out_5944879704050977948[19] = state[19]; - out_5944879704050977948[20] = state[20]; - out_5944879704050977948[21] = state[21]; +void f_fun(double *state, double dt, double *out_3547963935686363060) { + out_3547963935686363060[0] = dt*state[7] + state[0]; + out_3547963935686363060[1] = dt*state[8] + state[1]; + out_3547963935686363060[2] = dt*state[9] + state[2]; + out_3547963935686363060[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3]; + out_3547963935686363060[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4]; + out_3547963935686363060[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5]; + out_3547963935686363060[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6]; + out_3547963935686363060[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]) + state[7]; + out_3547963935686363060[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[8]; + out_3547963935686363060[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]) + state[9]; + out_3547963935686363060[10] = state[10]; + out_3547963935686363060[11] = state[11]; + out_3547963935686363060[12] = state[12]; + out_3547963935686363060[13] = state[13]; + out_3547963935686363060[14] = state[14]; + out_3547963935686363060[15] = state[15]; + out_3547963935686363060[16] = state[16]; + out_3547963935686363060[17] = state[17]; + out_3547963935686363060[18] = state[18]; + out_3547963935686363060[19] = state[19]; + out_3547963935686363060[20] = state[20]; + out_3547963935686363060[21] = state[21]; } -void F_fun(double *state, double dt, double *out_1511312995606333672) { - out_1511312995606333672[0] = 1; - out_1511312995606333672[1] = 0; - out_1511312995606333672[2] = 0; - out_1511312995606333672[3] = 0; - out_1511312995606333672[4] = 0; - out_1511312995606333672[5] = 0; - out_1511312995606333672[6] = dt; - out_1511312995606333672[7] = 0; - out_1511312995606333672[8] = 0; - out_1511312995606333672[9] = 0; - out_1511312995606333672[10] = 0; - out_1511312995606333672[11] = 0; - out_1511312995606333672[12] = 0; - out_1511312995606333672[13] = 0; - out_1511312995606333672[14] = 0; - out_1511312995606333672[15] = 0; - out_1511312995606333672[16] = 0; - out_1511312995606333672[17] = 0; - out_1511312995606333672[18] = 0; - out_1511312995606333672[19] = 0; - out_1511312995606333672[20] = 0; - out_1511312995606333672[21] = 0; - out_1511312995606333672[22] = 1; - out_1511312995606333672[23] = 0; - out_1511312995606333672[24] = 0; - out_1511312995606333672[25] = 0; - out_1511312995606333672[26] = 0; - out_1511312995606333672[27] = 0; - out_1511312995606333672[28] = dt; - out_1511312995606333672[29] = 0; - out_1511312995606333672[30] = 0; - out_1511312995606333672[31] = 0; - out_1511312995606333672[32] = 0; - out_1511312995606333672[33] = 0; - out_1511312995606333672[34] = 0; - out_1511312995606333672[35] = 0; - out_1511312995606333672[36] = 0; - out_1511312995606333672[37] = 0; - out_1511312995606333672[38] = 0; - out_1511312995606333672[39] = 0; - out_1511312995606333672[40] = 0; - out_1511312995606333672[41] = 0; - out_1511312995606333672[42] = 0; - out_1511312995606333672[43] = 0; - out_1511312995606333672[44] = 1; - out_1511312995606333672[45] = 0; - out_1511312995606333672[46] = 0; - out_1511312995606333672[47] = 0; - out_1511312995606333672[48] = 0; - out_1511312995606333672[49] = 0; - out_1511312995606333672[50] = dt; - out_1511312995606333672[51] = 0; - out_1511312995606333672[52] = 0; - out_1511312995606333672[53] = 0; - out_1511312995606333672[54] = 0; - out_1511312995606333672[55] = 0; - out_1511312995606333672[56] = 0; - out_1511312995606333672[57] = 0; - out_1511312995606333672[58] = 0; - out_1511312995606333672[59] = 0; - out_1511312995606333672[60] = 0; - out_1511312995606333672[61] = 0; - out_1511312995606333672[62] = 0; - out_1511312995606333672[63] = 0; - out_1511312995606333672[64] = 0; - out_1511312995606333672[65] = 0; - out_1511312995606333672[66] = 1; - out_1511312995606333672[67] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); - out_1511312995606333672[68] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]); - out_1511312995606333672[69] = 0; - out_1511312995606333672[70] = 0; - out_1511312995606333672[71] = 0; - out_1511312995606333672[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); - out_1511312995606333672[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); - out_1511312995606333672[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); - out_1511312995606333672[75] = 0; - out_1511312995606333672[76] = 0; - out_1511312995606333672[77] = 0; - out_1511312995606333672[78] = 0; - out_1511312995606333672[79] = 0; - out_1511312995606333672[80] = 0; - out_1511312995606333672[81] = 0; - out_1511312995606333672[82] = 0; - out_1511312995606333672[83] = 0; - out_1511312995606333672[84] = 0; - out_1511312995606333672[85] = 0; - out_1511312995606333672[86] = 0; - out_1511312995606333672[87] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); - out_1511312995606333672[88] = 1; - out_1511312995606333672[89] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]); - out_1511312995606333672[90] = 0; - out_1511312995606333672[91] = 0; - out_1511312995606333672[92] = 0; - out_1511312995606333672[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); - out_1511312995606333672[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); - out_1511312995606333672[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); - out_1511312995606333672[96] = 0; - out_1511312995606333672[97] = 0; - out_1511312995606333672[98] = 0; - out_1511312995606333672[99] = 0; - out_1511312995606333672[100] = 0; - out_1511312995606333672[101] = 0; - out_1511312995606333672[102] = 0; - out_1511312995606333672[103] = 0; - out_1511312995606333672[104] = 0; - out_1511312995606333672[105] = 0; - out_1511312995606333672[106] = 0; - out_1511312995606333672[107] = 0; - out_1511312995606333672[108] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]); - out_1511312995606333672[109] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]); - out_1511312995606333672[110] = 1; - out_1511312995606333672[111] = 0; - out_1511312995606333672[112] = 0; - out_1511312995606333672[113] = 0; - out_1511312995606333672[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); - out_1511312995606333672[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); - out_1511312995606333672[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); - out_1511312995606333672[117] = 0; - out_1511312995606333672[118] = 0; - out_1511312995606333672[119] = 0; - out_1511312995606333672[120] = 0; - out_1511312995606333672[121] = 0; - out_1511312995606333672[122] = 0; - out_1511312995606333672[123] = 0; - out_1511312995606333672[124] = 0; - out_1511312995606333672[125] = 0; - out_1511312995606333672[126] = 0; - out_1511312995606333672[127] = 0; - out_1511312995606333672[128] = 0; - out_1511312995606333672[129] = 0; - out_1511312995606333672[130] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); - out_1511312995606333672[131] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[18] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[16] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[17]); - out_1511312995606333672[132] = 1; - out_1511312995606333672[133] = 0; - out_1511312995606333672[134] = 0; - out_1511312995606333672[135] = 0; - out_1511312995606333672[136] = 0; - out_1511312995606333672[137] = 0; - out_1511312995606333672[138] = 0; - out_1511312995606333672[139] = 0; - out_1511312995606333672[140] = 0; - out_1511312995606333672[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); - out_1511312995606333672[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); - out_1511312995606333672[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); - out_1511312995606333672[144] = 0; - out_1511312995606333672[145] = 0; - out_1511312995606333672[146] = 0; - out_1511312995606333672[147] = 0; - out_1511312995606333672[148] = 0; - out_1511312995606333672[149] = 0; - out_1511312995606333672[150] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[17] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); - out_1511312995606333672[151] = 0; - out_1511312995606333672[152] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]); - out_1511312995606333672[153] = 0; - out_1511312995606333672[154] = 1; - out_1511312995606333672[155] = 0; - out_1511312995606333672[156] = 0; - out_1511312995606333672[157] = 0; - out_1511312995606333672[158] = 0; - out_1511312995606333672[159] = 0; - out_1511312995606333672[160] = 0; - out_1511312995606333672[161] = 0; - out_1511312995606333672[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); - out_1511312995606333672[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); - out_1511312995606333672[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); - out_1511312995606333672[165] = 0; - out_1511312995606333672[166] = 0; - out_1511312995606333672[167] = 0; - out_1511312995606333672[168] = 0; - out_1511312995606333672[169] = 0; - out_1511312995606333672[170] = 0; - out_1511312995606333672[171] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]); - out_1511312995606333672[172] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[18] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[16]); - out_1511312995606333672[173] = 0; - out_1511312995606333672[174] = 0; - out_1511312995606333672[175] = 0; - out_1511312995606333672[176] = 1; - out_1511312995606333672[177] = 0; - out_1511312995606333672[178] = 0; - out_1511312995606333672[179] = 0; - out_1511312995606333672[180] = 0; - out_1511312995606333672[181] = 0; - out_1511312995606333672[182] = 0; - out_1511312995606333672[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); - out_1511312995606333672[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); - out_1511312995606333672[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); - out_1511312995606333672[186] = 0; - out_1511312995606333672[187] = 0; - out_1511312995606333672[188] = 0; - out_1511312995606333672[189] = 0; - out_1511312995606333672[190] = 0; - out_1511312995606333672[191] = 0; - out_1511312995606333672[192] = 0; - out_1511312995606333672[193] = 0; - out_1511312995606333672[194] = 0; - out_1511312995606333672[195] = 0; - out_1511312995606333672[196] = 0; - out_1511312995606333672[197] = 0; - out_1511312995606333672[198] = 1; - out_1511312995606333672[199] = 0; - out_1511312995606333672[200] = 0; - out_1511312995606333672[201] = 0; - out_1511312995606333672[202] = 0; - out_1511312995606333672[203] = 0; - out_1511312995606333672[204] = 0; - out_1511312995606333672[205] = 0; - out_1511312995606333672[206] = 0; - out_1511312995606333672[207] = 0; - out_1511312995606333672[208] = 0; - out_1511312995606333672[209] = 0; - out_1511312995606333672[210] = 0; - out_1511312995606333672[211] = 0; - out_1511312995606333672[212] = 0; - out_1511312995606333672[213] = 0; - out_1511312995606333672[214] = 0; - out_1511312995606333672[215] = 0; - out_1511312995606333672[216] = 0; - out_1511312995606333672[217] = 0; - out_1511312995606333672[218] = 0; - out_1511312995606333672[219] = 0; - out_1511312995606333672[220] = 1; - out_1511312995606333672[221] = 0; - out_1511312995606333672[222] = 0; - out_1511312995606333672[223] = 0; - out_1511312995606333672[224] = 0; - out_1511312995606333672[225] = 0; - out_1511312995606333672[226] = 0; - out_1511312995606333672[227] = 0; - out_1511312995606333672[228] = 0; - out_1511312995606333672[229] = 0; - out_1511312995606333672[230] = 0; - out_1511312995606333672[231] = 0; - out_1511312995606333672[232] = 0; - out_1511312995606333672[233] = 0; - out_1511312995606333672[234] = 0; - out_1511312995606333672[235] = 0; - out_1511312995606333672[236] = 0; - out_1511312995606333672[237] = 0; - out_1511312995606333672[238] = 0; - out_1511312995606333672[239] = 0; - out_1511312995606333672[240] = 0; - out_1511312995606333672[241] = 0; - out_1511312995606333672[242] = 1; - out_1511312995606333672[243] = 0; - out_1511312995606333672[244] = 0; - out_1511312995606333672[245] = 0; - out_1511312995606333672[246] = 0; - out_1511312995606333672[247] = 0; - out_1511312995606333672[248] = 0; - out_1511312995606333672[249] = 0; - out_1511312995606333672[250] = 0; - out_1511312995606333672[251] = 0; - out_1511312995606333672[252] = 0; - out_1511312995606333672[253] = 0; - out_1511312995606333672[254] = 0; - out_1511312995606333672[255] = 0; - out_1511312995606333672[256] = 0; - out_1511312995606333672[257] = 0; - out_1511312995606333672[258] = 0; - out_1511312995606333672[259] = 0; - out_1511312995606333672[260] = 0; - out_1511312995606333672[261] = 0; - out_1511312995606333672[262] = 0; - out_1511312995606333672[263] = 0; - out_1511312995606333672[264] = 1; - out_1511312995606333672[265] = 0; - out_1511312995606333672[266] = 0; - out_1511312995606333672[267] = 0; - out_1511312995606333672[268] = 0; - out_1511312995606333672[269] = 0; - out_1511312995606333672[270] = 0; - out_1511312995606333672[271] = 0; - out_1511312995606333672[272] = 0; - out_1511312995606333672[273] = 0; - out_1511312995606333672[274] = 0; - out_1511312995606333672[275] = 0; - out_1511312995606333672[276] = 0; - out_1511312995606333672[277] = 0; - out_1511312995606333672[278] = 0; - out_1511312995606333672[279] = 0; - out_1511312995606333672[280] = 0; - out_1511312995606333672[281] = 0; - out_1511312995606333672[282] = 0; - out_1511312995606333672[283] = 0; - out_1511312995606333672[284] = 0; - out_1511312995606333672[285] = 0; - out_1511312995606333672[286] = 1; - out_1511312995606333672[287] = 0; - out_1511312995606333672[288] = 0; - out_1511312995606333672[289] = 0; - out_1511312995606333672[290] = 0; - out_1511312995606333672[291] = 0; - out_1511312995606333672[292] = 0; - out_1511312995606333672[293] = 0; - out_1511312995606333672[294] = 0; - out_1511312995606333672[295] = 0; - out_1511312995606333672[296] = 0; - out_1511312995606333672[297] = 0; - out_1511312995606333672[298] = 0; - out_1511312995606333672[299] = 0; - out_1511312995606333672[300] = 0; - out_1511312995606333672[301] = 0; - out_1511312995606333672[302] = 0; - out_1511312995606333672[303] = 0; - out_1511312995606333672[304] = 0; - out_1511312995606333672[305] = 0; - out_1511312995606333672[306] = 0; - out_1511312995606333672[307] = 0; - out_1511312995606333672[308] = 1; - out_1511312995606333672[309] = 0; - out_1511312995606333672[310] = 0; - out_1511312995606333672[311] = 0; - out_1511312995606333672[312] = 0; - out_1511312995606333672[313] = 0; - out_1511312995606333672[314] = 0; - out_1511312995606333672[315] = 0; - out_1511312995606333672[316] = 0; - out_1511312995606333672[317] = 0; - out_1511312995606333672[318] = 0; - out_1511312995606333672[319] = 0; - out_1511312995606333672[320] = 0; - out_1511312995606333672[321] = 0; - out_1511312995606333672[322] = 0; - out_1511312995606333672[323] = 0; - out_1511312995606333672[324] = 0; - out_1511312995606333672[325] = 0; - out_1511312995606333672[326] = 0; - out_1511312995606333672[327] = 0; - out_1511312995606333672[328] = 0; - out_1511312995606333672[329] = 0; - out_1511312995606333672[330] = 1; - out_1511312995606333672[331] = 0; - out_1511312995606333672[332] = 0; - out_1511312995606333672[333] = 0; - out_1511312995606333672[334] = 0; - out_1511312995606333672[335] = 0; - out_1511312995606333672[336] = 0; - out_1511312995606333672[337] = 0; - out_1511312995606333672[338] = 0; - out_1511312995606333672[339] = 0; - out_1511312995606333672[340] = 0; - out_1511312995606333672[341] = 0; - out_1511312995606333672[342] = 0; - out_1511312995606333672[343] = 0; - out_1511312995606333672[344] = 0; - out_1511312995606333672[345] = 0; - out_1511312995606333672[346] = 0; - out_1511312995606333672[347] = 0; - out_1511312995606333672[348] = 0; - out_1511312995606333672[349] = 0; - out_1511312995606333672[350] = 0; - out_1511312995606333672[351] = 0; - out_1511312995606333672[352] = 1; - out_1511312995606333672[353] = 0; - out_1511312995606333672[354] = 0; - out_1511312995606333672[355] = 0; - out_1511312995606333672[356] = 0; - out_1511312995606333672[357] = 0; - out_1511312995606333672[358] = 0; - out_1511312995606333672[359] = 0; - out_1511312995606333672[360] = 0; - out_1511312995606333672[361] = 0; - out_1511312995606333672[362] = 0; - out_1511312995606333672[363] = 0; - out_1511312995606333672[364] = 0; - out_1511312995606333672[365] = 0; - out_1511312995606333672[366] = 0; - out_1511312995606333672[367] = 0; - out_1511312995606333672[368] = 0; - out_1511312995606333672[369] = 0; - out_1511312995606333672[370] = 0; - out_1511312995606333672[371] = 0; - out_1511312995606333672[372] = 0; - out_1511312995606333672[373] = 0; - out_1511312995606333672[374] = 1; - out_1511312995606333672[375] = 0; - out_1511312995606333672[376] = 0; - out_1511312995606333672[377] = 0; - out_1511312995606333672[378] = 0; - out_1511312995606333672[379] = 0; - out_1511312995606333672[380] = 0; - out_1511312995606333672[381] = 0; - out_1511312995606333672[382] = 0; - out_1511312995606333672[383] = 0; - out_1511312995606333672[384] = 0; - out_1511312995606333672[385] = 0; - out_1511312995606333672[386] = 0; - out_1511312995606333672[387] = 0; - out_1511312995606333672[388] = 0; - out_1511312995606333672[389] = 0; - out_1511312995606333672[390] = 0; - out_1511312995606333672[391] = 0; - out_1511312995606333672[392] = 0; - out_1511312995606333672[393] = 0; - out_1511312995606333672[394] = 0; - out_1511312995606333672[395] = 0; - out_1511312995606333672[396] = 1; - out_1511312995606333672[397] = 0; - out_1511312995606333672[398] = 0; - out_1511312995606333672[399] = 0; - out_1511312995606333672[400] = 0; - out_1511312995606333672[401] = 0; - out_1511312995606333672[402] = 0; - out_1511312995606333672[403] = 0; - out_1511312995606333672[404] = 0; - out_1511312995606333672[405] = 0; - out_1511312995606333672[406] = 0; - out_1511312995606333672[407] = 0; - out_1511312995606333672[408] = 0; - out_1511312995606333672[409] = 0; - out_1511312995606333672[410] = 0; - out_1511312995606333672[411] = 0; - out_1511312995606333672[412] = 0; - out_1511312995606333672[413] = 0; - out_1511312995606333672[414] = 0; - out_1511312995606333672[415] = 0; - out_1511312995606333672[416] = 0; - out_1511312995606333672[417] = 0; - out_1511312995606333672[418] = 1; - out_1511312995606333672[419] = 0; - out_1511312995606333672[420] = 0; - out_1511312995606333672[421] = 0; - out_1511312995606333672[422] = 0; - out_1511312995606333672[423] = 0; - out_1511312995606333672[424] = 0; - out_1511312995606333672[425] = 0; - out_1511312995606333672[426] = 0; - out_1511312995606333672[427] = 0; - out_1511312995606333672[428] = 0; - out_1511312995606333672[429] = 0; - out_1511312995606333672[430] = 0; - out_1511312995606333672[431] = 0; - out_1511312995606333672[432] = 0; - out_1511312995606333672[433] = 0; - out_1511312995606333672[434] = 0; - out_1511312995606333672[435] = 0; - out_1511312995606333672[436] = 0; - out_1511312995606333672[437] = 0; - out_1511312995606333672[438] = 0; - out_1511312995606333672[439] = 0; - out_1511312995606333672[440] = 1; +void F_fun(double *state, double dt, double *out_3254137990445528781) { + out_3254137990445528781[0] = 1; + out_3254137990445528781[1] = 0; + out_3254137990445528781[2] = 0; + out_3254137990445528781[3] = 0; + out_3254137990445528781[4] = 0; + out_3254137990445528781[5] = 0; + out_3254137990445528781[6] = dt; + out_3254137990445528781[7] = 0; + out_3254137990445528781[8] = 0; + out_3254137990445528781[9] = 0; + out_3254137990445528781[10] = 0; + out_3254137990445528781[11] = 0; + out_3254137990445528781[12] = 0; + out_3254137990445528781[13] = 0; + out_3254137990445528781[14] = 0; + out_3254137990445528781[15] = 0; + out_3254137990445528781[16] = 0; + out_3254137990445528781[17] = 0; + out_3254137990445528781[18] = 0; + out_3254137990445528781[19] = 0; + out_3254137990445528781[20] = 0; + out_3254137990445528781[21] = 0; + out_3254137990445528781[22] = 1; + out_3254137990445528781[23] = 0; + out_3254137990445528781[24] = 0; + out_3254137990445528781[25] = 0; + out_3254137990445528781[26] = 0; + out_3254137990445528781[27] = 0; + out_3254137990445528781[28] = dt; + out_3254137990445528781[29] = 0; + out_3254137990445528781[30] = 0; + out_3254137990445528781[31] = 0; + out_3254137990445528781[32] = 0; + out_3254137990445528781[33] = 0; + out_3254137990445528781[34] = 0; + out_3254137990445528781[35] = 0; + out_3254137990445528781[36] = 0; + out_3254137990445528781[37] = 0; + out_3254137990445528781[38] = 0; + out_3254137990445528781[39] = 0; + out_3254137990445528781[40] = 0; + out_3254137990445528781[41] = 0; + out_3254137990445528781[42] = 0; + out_3254137990445528781[43] = 0; + out_3254137990445528781[44] = 1; + out_3254137990445528781[45] = 0; + out_3254137990445528781[46] = 0; + out_3254137990445528781[47] = 0; + out_3254137990445528781[48] = 0; + out_3254137990445528781[49] = 0; + out_3254137990445528781[50] = dt; + out_3254137990445528781[51] = 0; + out_3254137990445528781[52] = 0; + out_3254137990445528781[53] = 0; + out_3254137990445528781[54] = 0; + out_3254137990445528781[55] = 0; + out_3254137990445528781[56] = 0; + out_3254137990445528781[57] = 0; + out_3254137990445528781[58] = 0; + out_3254137990445528781[59] = 0; + out_3254137990445528781[60] = 0; + out_3254137990445528781[61] = 0; + out_3254137990445528781[62] = 0; + out_3254137990445528781[63] = 0; + out_3254137990445528781[64] = 0; + out_3254137990445528781[65] = 0; + out_3254137990445528781[66] = 1; + out_3254137990445528781[67] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); + out_3254137990445528781[68] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]); + out_3254137990445528781[69] = 0; + out_3254137990445528781[70] = 0; + out_3254137990445528781[71] = 0; + out_3254137990445528781[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); + out_3254137990445528781[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); + out_3254137990445528781[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); + out_3254137990445528781[75] = 0; + out_3254137990445528781[76] = 0; + out_3254137990445528781[77] = 0; + out_3254137990445528781[78] = 0; + out_3254137990445528781[79] = 0; + out_3254137990445528781[80] = 0; + out_3254137990445528781[81] = 0; + out_3254137990445528781[82] = 0; + out_3254137990445528781[83] = 0; + out_3254137990445528781[84] = 0; + out_3254137990445528781[85] = 0; + out_3254137990445528781[86] = 0; + out_3254137990445528781[87] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); + out_3254137990445528781[88] = 1; + out_3254137990445528781[89] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]); + out_3254137990445528781[90] = 0; + out_3254137990445528781[91] = 0; + out_3254137990445528781[92] = 0; + out_3254137990445528781[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); + out_3254137990445528781[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); + out_3254137990445528781[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); + out_3254137990445528781[96] = 0; + out_3254137990445528781[97] = 0; + out_3254137990445528781[98] = 0; + out_3254137990445528781[99] = 0; + out_3254137990445528781[100] = 0; + out_3254137990445528781[101] = 0; + out_3254137990445528781[102] = 0; + out_3254137990445528781[103] = 0; + out_3254137990445528781[104] = 0; + out_3254137990445528781[105] = 0; + out_3254137990445528781[106] = 0; + out_3254137990445528781[107] = 0; + out_3254137990445528781[108] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]); + out_3254137990445528781[109] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]); + out_3254137990445528781[110] = 1; + out_3254137990445528781[111] = 0; + out_3254137990445528781[112] = 0; + out_3254137990445528781[113] = 0; + out_3254137990445528781[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); + out_3254137990445528781[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); + out_3254137990445528781[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); + out_3254137990445528781[117] = 0; + out_3254137990445528781[118] = 0; + out_3254137990445528781[119] = 0; + out_3254137990445528781[120] = 0; + out_3254137990445528781[121] = 0; + out_3254137990445528781[122] = 0; + out_3254137990445528781[123] = 0; + out_3254137990445528781[124] = 0; + out_3254137990445528781[125] = 0; + out_3254137990445528781[126] = 0; + out_3254137990445528781[127] = 0; + out_3254137990445528781[128] = 0; + out_3254137990445528781[129] = 0; + out_3254137990445528781[130] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); + out_3254137990445528781[131] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[18] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[16] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[17]); + out_3254137990445528781[132] = 1; + out_3254137990445528781[133] = 0; + out_3254137990445528781[134] = 0; + out_3254137990445528781[135] = 0; + out_3254137990445528781[136] = 0; + out_3254137990445528781[137] = 0; + out_3254137990445528781[138] = 0; + out_3254137990445528781[139] = 0; + out_3254137990445528781[140] = 0; + out_3254137990445528781[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); + out_3254137990445528781[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); + out_3254137990445528781[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); + out_3254137990445528781[144] = 0; + out_3254137990445528781[145] = 0; + out_3254137990445528781[146] = 0; + out_3254137990445528781[147] = 0; + out_3254137990445528781[148] = 0; + out_3254137990445528781[149] = 0; + out_3254137990445528781[150] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[17] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); + out_3254137990445528781[151] = 0; + out_3254137990445528781[152] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]); + out_3254137990445528781[153] = 0; + out_3254137990445528781[154] = 1; + out_3254137990445528781[155] = 0; + out_3254137990445528781[156] = 0; + out_3254137990445528781[157] = 0; + out_3254137990445528781[158] = 0; + out_3254137990445528781[159] = 0; + out_3254137990445528781[160] = 0; + out_3254137990445528781[161] = 0; + out_3254137990445528781[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); + out_3254137990445528781[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); + out_3254137990445528781[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); + out_3254137990445528781[165] = 0; + out_3254137990445528781[166] = 0; + out_3254137990445528781[167] = 0; + out_3254137990445528781[168] = 0; + out_3254137990445528781[169] = 0; + out_3254137990445528781[170] = 0; + out_3254137990445528781[171] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]); + out_3254137990445528781[172] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[18] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[16]); + out_3254137990445528781[173] = 0; + out_3254137990445528781[174] = 0; + out_3254137990445528781[175] = 0; + out_3254137990445528781[176] = 1; + out_3254137990445528781[177] = 0; + out_3254137990445528781[178] = 0; + out_3254137990445528781[179] = 0; + out_3254137990445528781[180] = 0; + out_3254137990445528781[181] = 0; + out_3254137990445528781[182] = 0; + out_3254137990445528781[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); + out_3254137990445528781[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); + out_3254137990445528781[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); + out_3254137990445528781[186] = 0; + out_3254137990445528781[187] = 0; + out_3254137990445528781[188] = 0; + out_3254137990445528781[189] = 0; + out_3254137990445528781[190] = 0; + out_3254137990445528781[191] = 0; + out_3254137990445528781[192] = 0; + out_3254137990445528781[193] = 0; + out_3254137990445528781[194] = 0; + out_3254137990445528781[195] = 0; + out_3254137990445528781[196] = 0; + out_3254137990445528781[197] = 0; + out_3254137990445528781[198] = 1; + out_3254137990445528781[199] = 0; + out_3254137990445528781[200] = 0; + out_3254137990445528781[201] = 0; + out_3254137990445528781[202] = 0; + out_3254137990445528781[203] = 0; + out_3254137990445528781[204] = 0; + out_3254137990445528781[205] = 0; + out_3254137990445528781[206] = 0; + out_3254137990445528781[207] = 0; + out_3254137990445528781[208] = 0; + out_3254137990445528781[209] = 0; + out_3254137990445528781[210] = 0; + out_3254137990445528781[211] = 0; + out_3254137990445528781[212] = 0; + out_3254137990445528781[213] = 0; + out_3254137990445528781[214] = 0; + out_3254137990445528781[215] = 0; + out_3254137990445528781[216] = 0; + out_3254137990445528781[217] = 0; + out_3254137990445528781[218] = 0; + out_3254137990445528781[219] = 0; + out_3254137990445528781[220] = 1; + out_3254137990445528781[221] = 0; + out_3254137990445528781[222] = 0; + out_3254137990445528781[223] = 0; + out_3254137990445528781[224] = 0; + out_3254137990445528781[225] = 0; + out_3254137990445528781[226] = 0; + out_3254137990445528781[227] = 0; + out_3254137990445528781[228] = 0; + out_3254137990445528781[229] = 0; + out_3254137990445528781[230] = 0; + out_3254137990445528781[231] = 0; + out_3254137990445528781[232] = 0; + out_3254137990445528781[233] = 0; + out_3254137990445528781[234] = 0; + out_3254137990445528781[235] = 0; + out_3254137990445528781[236] = 0; + out_3254137990445528781[237] = 0; + out_3254137990445528781[238] = 0; + out_3254137990445528781[239] = 0; + out_3254137990445528781[240] = 0; + out_3254137990445528781[241] = 0; + out_3254137990445528781[242] = 1; + out_3254137990445528781[243] = 0; + out_3254137990445528781[244] = 0; + out_3254137990445528781[245] = 0; + out_3254137990445528781[246] = 0; + out_3254137990445528781[247] = 0; + out_3254137990445528781[248] = 0; + out_3254137990445528781[249] = 0; + out_3254137990445528781[250] = 0; + out_3254137990445528781[251] = 0; + out_3254137990445528781[252] = 0; + out_3254137990445528781[253] = 0; + out_3254137990445528781[254] = 0; + out_3254137990445528781[255] = 0; + out_3254137990445528781[256] = 0; + out_3254137990445528781[257] = 0; + out_3254137990445528781[258] = 0; + out_3254137990445528781[259] = 0; + out_3254137990445528781[260] = 0; + out_3254137990445528781[261] = 0; + out_3254137990445528781[262] = 0; + out_3254137990445528781[263] = 0; + out_3254137990445528781[264] = 1; + out_3254137990445528781[265] = 0; + out_3254137990445528781[266] = 0; + out_3254137990445528781[267] = 0; + out_3254137990445528781[268] = 0; + out_3254137990445528781[269] = 0; + out_3254137990445528781[270] = 0; + out_3254137990445528781[271] = 0; + out_3254137990445528781[272] = 0; + out_3254137990445528781[273] = 0; + out_3254137990445528781[274] = 0; + out_3254137990445528781[275] = 0; + out_3254137990445528781[276] = 0; + out_3254137990445528781[277] = 0; + out_3254137990445528781[278] = 0; + out_3254137990445528781[279] = 0; + out_3254137990445528781[280] = 0; + out_3254137990445528781[281] = 0; + out_3254137990445528781[282] = 0; + out_3254137990445528781[283] = 0; + out_3254137990445528781[284] = 0; + out_3254137990445528781[285] = 0; + out_3254137990445528781[286] = 1; + out_3254137990445528781[287] = 0; + out_3254137990445528781[288] = 0; + out_3254137990445528781[289] = 0; + out_3254137990445528781[290] = 0; + out_3254137990445528781[291] = 0; + out_3254137990445528781[292] = 0; + out_3254137990445528781[293] = 0; + out_3254137990445528781[294] = 0; + out_3254137990445528781[295] = 0; + out_3254137990445528781[296] = 0; + out_3254137990445528781[297] = 0; + out_3254137990445528781[298] = 0; + out_3254137990445528781[299] = 0; + out_3254137990445528781[300] = 0; + out_3254137990445528781[301] = 0; + out_3254137990445528781[302] = 0; + out_3254137990445528781[303] = 0; + out_3254137990445528781[304] = 0; + out_3254137990445528781[305] = 0; + out_3254137990445528781[306] = 0; + out_3254137990445528781[307] = 0; + out_3254137990445528781[308] = 1; + out_3254137990445528781[309] = 0; + out_3254137990445528781[310] = 0; + out_3254137990445528781[311] = 0; + out_3254137990445528781[312] = 0; + out_3254137990445528781[313] = 0; + out_3254137990445528781[314] = 0; + out_3254137990445528781[315] = 0; + out_3254137990445528781[316] = 0; + out_3254137990445528781[317] = 0; + out_3254137990445528781[318] = 0; + out_3254137990445528781[319] = 0; + out_3254137990445528781[320] = 0; + out_3254137990445528781[321] = 0; + out_3254137990445528781[322] = 0; + out_3254137990445528781[323] = 0; + out_3254137990445528781[324] = 0; + out_3254137990445528781[325] = 0; + out_3254137990445528781[326] = 0; + out_3254137990445528781[327] = 0; + out_3254137990445528781[328] = 0; + out_3254137990445528781[329] = 0; + out_3254137990445528781[330] = 1; + out_3254137990445528781[331] = 0; + out_3254137990445528781[332] = 0; + out_3254137990445528781[333] = 0; + out_3254137990445528781[334] = 0; + out_3254137990445528781[335] = 0; + out_3254137990445528781[336] = 0; + out_3254137990445528781[337] = 0; + out_3254137990445528781[338] = 0; + out_3254137990445528781[339] = 0; + out_3254137990445528781[340] = 0; + out_3254137990445528781[341] = 0; + out_3254137990445528781[342] = 0; + out_3254137990445528781[343] = 0; + out_3254137990445528781[344] = 0; + out_3254137990445528781[345] = 0; + out_3254137990445528781[346] = 0; + out_3254137990445528781[347] = 0; + out_3254137990445528781[348] = 0; + out_3254137990445528781[349] = 0; + out_3254137990445528781[350] = 0; + out_3254137990445528781[351] = 0; + out_3254137990445528781[352] = 1; + out_3254137990445528781[353] = 0; + out_3254137990445528781[354] = 0; + out_3254137990445528781[355] = 0; + out_3254137990445528781[356] = 0; + out_3254137990445528781[357] = 0; + out_3254137990445528781[358] = 0; + out_3254137990445528781[359] = 0; + out_3254137990445528781[360] = 0; + out_3254137990445528781[361] = 0; + out_3254137990445528781[362] = 0; + out_3254137990445528781[363] = 0; + out_3254137990445528781[364] = 0; + out_3254137990445528781[365] = 0; + out_3254137990445528781[366] = 0; + out_3254137990445528781[367] = 0; + out_3254137990445528781[368] = 0; + out_3254137990445528781[369] = 0; + out_3254137990445528781[370] = 0; + out_3254137990445528781[371] = 0; + out_3254137990445528781[372] = 0; + out_3254137990445528781[373] = 0; + out_3254137990445528781[374] = 1; + out_3254137990445528781[375] = 0; + out_3254137990445528781[376] = 0; + out_3254137990445528781[377] = 0; + out_3254137990445528781[378] = 0; + out_3254137990445528781[379] = 0; + out_3254137990445528781[380] = 0; + out_3254137990445528781[381] = 0; + out_3254137990445528781[382] = 0; + out_3254137990445528781[383] = 0; + out_3254137990445528781[384] = 0; + out_3254137990445528781[385] = 0; + out_3254137990445528781[386] = 0; + out_3254137990445528781[387] = 0; + out_3254137990445528781[388] = 0; + out_3254137990445528781[389] = 0; + out_3254137990445528781[390] = 0; + out_3254137990445528781[391] = 0; + out_3254137990445528781[392] = 0; + out_3254137990445528781[393] = 0; + out_3254137990445528781[394] = 0; + out_3254137990445528781[395] = 0; + out_3254137990445528781[396] = 1; + out_3254137990445528781[397] = 0; + out_3254137990445528781[398] = 0; + out_3254137990445528781[399] = 0; + out_3254137990445528781[400] = 0; + out_3254137990445528781[401] = 0; + out_3254137990445528781[402] = 0; + out_3254137990445528781[403] = 0; + out_3254137990445528781[404] = 0; + out_3254137990445528781[405] = 0; + out_3254137990445528781[406] = 0; + out_3254137990445528781[407] = 0; + out_3254137990445528781[408] = 0; + out_3254137990445528781[409] = 0; + out_3254137990445528781[410] = 0; + out_3254137990445528781[411] = 0; + out_3254137990445528781[412] = 0; + out_3254137990445528781[413] = 0; + out_3254137990445528781[414] = 0; + out_3254137990445528781[415] = 0; + out_3254137990445528781[416] = 0; + out_3254137990445528781[417] = 0; + out_3254137990445528781[418] = 1; + out_3254137990445528781[419] = 0; + out_3254137990445528781[420] = 0; + out_3254137990445528781[421] = 0; + out_3254137990445528781[422] = 0; + out_3254137990445528781[423] = 0; + out_3254137990445528781[424] = 0; + out_3254137990445528781[425] = 0; + out_3254137990445528781[426] = 0; + out_3254137990445528781[427] = 0; + out_3254137990445528781[428] = 0; + out_3254137990445528781[429] = 0; + out_3254137990445528781[430] = 0; + out_3254137990445528781[431] = 0; + out_3254137990445528781[432] = 0; + out_3254137990445528781[433] = 0; + out_3254137990445528781[434] = 0; + out_3254137990445528781[435] = 0; + out_3254137990445528781[436] = 0; + out_3254137990445528781[437] = 0; + out_3254137990445528781[438] = 0; + out_3254137990445528781[439] = 0; + out_3254137990445528781[440] = 1; } -void h_4(double *state, double *unused, double *out_6748587584352144008) { - out_6748587584352144008[0] = state[10] + state[13]; - out_6748587584352144008[1] = state[11] + state[14]; - out_6748587584352144008[2] = state[12] + state[15]; +void h_4(double *state, double *unused, double *out_4410787759120580449) { + out_4410787759120580449[0] = state[10] + state[13]; + out_4410787759120580449[1] = state[11] + state[14]; + out_4410787759120580449[2] = state[12] + state[15]; } -void H_4(double *state, double *unused, double *out_6619224273712010162) { - out_6619224273712010162[0] = 0; - out_6619224273712010162[1] = 0; - out_6619224273712010162[2] = 0; - out_6619224273712010162[3] = 0; - out_6619224273712010162[4] = 0; - out_6619224273712010162[5] = 0; - out_6619224273712010162[6] = 0; - out_6619224273712010162[7] = 0; - out_6619224273712010162[8] = 0; - out_6619224273712010162[9] = 0; - out_6619224273712010162[10] = 1; - out_6619224273712010162[11] = 0; - out_6619224273712010162[12] = 0; - out_6619224273712010162[13] = 1; - out_6619224273712010162[14] = 0; - out_6619224273712010162[15] = 0; - out_6619224273712010162[16] = 0; - out_6619224273712010162[17] = 0; - out_6619224273712010162[18] = 0; - out_6619224273712010162[19] = 0; - out_6619224273712010162[20] = 0; - out_6619224273712010162[21] = 0; - out_6619224273712010162[22] = 0; - out_6619224273712010162[23] = 0; - out_6619224273712010162[24] = 0; - out_6619224273712010162[25] = 0; - out_6619224273712010162[26] = 0; - out_6619224273712010162[27] = 0; - out_6619224273712010162[28] = 0; - out_6619224273712010162[29] = 0; - out_6619224273712010162[30] = 0; - out_6619224273712010162[31] = 0; - out_6619224273712010162[32] = 0; - out_6619224273712010162[33] = 1; - out_6619224273712010162[34] = 0; - out_6619224273712010162[35] = 0; - out_6619224273712010162[36] = 1; - out_6619224273712010162[37] = 0; - out_6619224273712010162[38] = 0; - out_6619224273712010162[39] = 0; - out_6619224273712010162[40] = 0; - out_6619224273712010162[41] = 0; - out_6619224273712010162[42] = 0; - out_6619224273712010162[43] = 0; - out_6619224273712010162[44] = 0; - out_6619224273712010162[45] = 0; - out_6619224273712010162[46] = 0; - out_6619224273712010162[47] = 0; - out_6619224273712010162[48] = 0; - out_6619224273712010162[49] = 0; - out_6619224273712010162[50] = 0; - out_6619224273712010162[51] = 0; - out_6619224273712010162[52] = 0; - out_6619224273712010162[53] = 0; - out_6619224273712010162[54] = 0; - out_6619224273712010162[55] = 0; - out_6619224273712010162[56] = 1; - out_6619224273712010162[57] = 0; - out_6619224273712010162[58] = 0; - out_6619224273712010162[59] = 1; - out_6619224273712010162[60] = 0; - out_6619224273712010162[61] = 0; - out_6619224273712010162[62] = 0; - out_6619224273712010162[63] = 0; - out_6619224273712010162[64] = 0; - out_6619224273712010162[65] = 0; +void H_4(double *state, double *unused, double *out_5512660098689845475) { + out_5512660098689845475[0] = 0; + out_5512660098689845475[1] = 0; + out_5512660098689845475[2] = 0; + out_5512660098689845475[3] = 0; + out_5512660098689845475[4] = 0; + out_5512660098689845475[5] = 0; + out_5512660098689845475[6] = 0; + out_5512660098689845475[7] = 0; + out_5512660098689845475[8] = 0; + out_5512660098689845475[9] = 0; + out_5512660098689845475[10] = 1; + out_5512660098689845475[11] = 0; + out_5512660098689845475[12] = 0; + out_5512660098689845475[13] = 1; + out_5512660098689845475[14] = 0; + out_5512660098689845475[15] = 0; + out_5512660098689845475[16] = 0; + out_5512660098689845475[17] = 0; + out_5512660098689845475[18] = 0; + out_5512660098689845475[19] = 0; + out_5512660098689845475[20] = 0; + out_5512660098689845475[21] = 0; + out_5512660098689845475[22] = 0; + out_5512660098689845475[23] = 0; + out_5512660098689845475[24] = 0; + out_5512660098689845475[25] = 0; + out_5512660098689845475[26] = 0; + out_5512660098689845475[27] = 0; + out_5512660098689845475[28] = 0; + out_5512660098689845475[29] = 0; + out_5512660098689845475[30] = 0; + out_5512660098689845475[31] = 0; + out_5512660098689845475[32] = 0; + out_5512660098689845475[33] = 1; + out_5512660098689845475[34] = 0; + out_5512660098689845475[35] = 0; + out_5512660098689845475[36] = 1; + out_5512660098689845475[37] = 0; + out_5512660098689845475[38] = 0; + out_5512660098689845475[39] = 0; + out_5512660098689845475[40] = 0; + out_5512660098689845475[41] = 0; + out_5512660098689845475[42] = 0; + out_5512660098689845475[43] = 0; + out_5512660098689845475[44] = 0; + out_5512660098689845475[45] = 0; + out_5512660098689845475[46] = 0; + out_5512660098689845475[47] = 0; + out_5512660098689845475[48] = 0; + out_5512660098689845475[49] = 0; + out_5512660098689845475[50] = 0; + out_5512660098689845475[51] = 0; + out_5512660098689845475[52] = 0; + out_5512660098689845475[53] = 0; + out_5512660098689845475[54] = 0; + out_5512660098689845475[55] = 0; + out_5512660098689845475[56] = 1; + out_5512660098689845475[57] = 0; + out_5512660098689845475[58] = 0; + out_5512660098689845475[59] = 1; + out_5512660098689845475[60] = 0; + out_5512660098689845475[61] = 0; + out_5512660098689845475[62] = 0; + out_5512660098689845475[63] = 0; + out_5512660098689845475[64] = 0; + out_5512660098689845475[65] = 0; } -void h_9(double *state, double *unused, double *out_8367559596262948470) { - out_8367559596262948470[0] = state[10]; - out_8367559596262948470[1] = state[11]; - out_8367559596262948470[2] = state[12]; +void h_9(double *state, double *unused, double *out_4264504783842197092) { + out_4264504783842197092[0] = state[10]; + out_4264504783842197092[1] = state[11]; + out_4264504783842197092[2] = state[12]; } -void H_9(double *state, double *unused, double *out_6860413920341600807) { - out_6860413920341600807[0] = 0; - out_6860413920341600807[1] = 0; - out_6860413920341600807[2] = 0; - out_6860413920341600807[3] = 0; - out_6860413920341600807[4] = 0; - out_6860413920341600807[5] = 0; - out_6860413920341600807[6] = 0; - out_6860413920341600807[7] = 0; - out_6860413920341600807[8] = 0; - out_6860413920341600807[9] = 0; - out_6860413920341600807[10] = 1; - out_6860413920341600807[11] = 0; - out_6860413920341600807[12] = 0; - out_6860413920341600807[13] = 0; - out_6860413920341600807[14] = 0; - out_6860413920341600807[15] = 0; - out_6860413920341600807[16] = 0; - out_6860413920341600807[17] = 0; - out_6860413920341600807[18] = 0; - out_6860413920341600807[19] = 0; - out_6860413920341600807[20] = 0; - out_6860413920341600807[21] = 0; - out_6860413920341600807[22] = 0; - out_6860413920341600807[23] = 0; - out_6860413920341600807[24] = 0; - out_6860413920341600807[25] = 0; - out_6860413920341600807[26] = 0; - out_6860413920341600807[27] = 0; - out_6860413920341600807[28] = 0; - out_6860413920341600807[29] = 0; - out_6860413920341600807[30] = 0; - out_6860413920341600807[31] = 0; - out_6860413920341600807[32] = 0; - out_6860413920341600807[33] = 1; - out_6860413920341600807[34] = 0; - out_6860413920341600807[35] = 0; - out_6860413920341600807[36] = 0; - out_6860413920341600807[37] = 0; - out_6860413920341600807[38] = 0; - out_6860413920341600807[39] = 0; - out_6860413920341600807[40] = 0; - out_6860413920341600807[41] = 0; - out_6860413920341600807[42] = 0; - out_6860413920341600807[43] = 0; - out_6860413920341600807[44] = 0; - out_6860413920341600807[45] = 0; - out_6860413920341600807[46] = 0; - out_6860413920341600807[47] = 0; - out_6860413920341600807[48] = 0; - out_6860413920341600807[49] = 0; - out_6860413920341600807[50] = 0; - out_6860413920341600807[51] = 0; - out_6860413920341600807[52] = 0; - out_6860413920341600807[53] = 0; - out_6860413920341600807[54] = 0; - out_6860413920341600807[55] = 0; - out_6860413920341600807[56] = 1; - out_6860413920341600807[57] = 0; - out_6860413920341600807[58] = 0; - out_6860413920341600807[59] = 0; - out_6860413920341600807[60] = 0; - out_6860413920341600807[61] = 0; - out_6860413920341600807[62] = 0; - out_6860413920341600807[63] = 0; - out_6860413920341600807[64] = 0; - out_6860413920341600807[65] = 0; +void H_9(double *state, double *unused, double *out_5271470452060254830) { + out_5271470452060254830[0] = 0; + out_5271470452060254830[1] = 0; + out_5271470452060254830[2] = 0; + out_5271470452060254830[3] = 0; + out_5271470452060254830[4] = 0; + out_5271470452060254830[5] = 0; + out_5271470452060254830[6] = 0; + out_5271470452060254830[7] = 0; + out_5271470452060254830[8] = 0; + out_5271470452060254830[9] = 0; + out_5271470452060254830[10] = 1; + out_5271470452060254830[11] = 0; + out_5271470452060254830[12] = 0; + out_5271470452060254830[13] = 0; + out_5271470452060254830[14] = 0; + out_5271470452060254830[15] = 0; + out_5271470452060254830[16] = 0; + out_5271470452060254830[17] = 0; + out_5271470452060254830[18] = 0; + out_5271470452060254830[19] = 0; + out_5271470452060254830[20] = 0; + out_5271470452060254830[21] = 0; + out_5271470452060254830[22] = 0; + out_5271470452060254830[23] = 0; + out_5271470452060254830[24] = 0; + out_5271470452060254830[25] = 0; + out_5271470452060254830[26] = 0; + out_5271470452060254830[27] = 0; + out_5271470452060254830[28] = 0; + out_5271470452060254830[29] = 0; + out_5271470452060254830[30] = 0; + out_5271470452060254830[31] = 0; + out_5271470452060254830[32] = 0; + out_5271470452060254830[33] = 1; + out_5271470452060254830[34] = 0; + out_5271470452060254830[35] = 0; + out_5271470452060254830[36] = 0; + out_5271470452060254830[37] = 0; + out_5271470452060254830[38] = 0; + out_5271470452060254830[39] = 0; + out_5271470452060254830[40] = 0; + out_5271470452060254830[41] = 0; + out_5271470452060254830[42] = 0; + out_5271470452060254830[43] = 0; + out_5271470452060254830[44] = 0; + out_5271470452060254830[45] = 0; + out_5271470452060254830[46] = 0; + out_5271470452060254830[47] = 0; + out_5271470452060254830[48] = 0; + out_5271470452060254830[49] = 0; + out_5271470452060254830[50] = 0; + out_5271470452060254830[51] = 0; + out_5271470452060254830[52] = 0; + out_5271470452060254830[53] = 0; + out_5271470452060254830[54] = 0; + out_5271470452060254830[55] = 0; + out_5271470452060254830[56] = 1; + out_5271470452060254830[57] = 0; + out_5271470452060254830[58] = 0; + out_5271470452060254830[59] = 0; + out_5271470452060254830[60] = 0; + out_5271470452060254830[61] = 0; + out_5271470452060254830[62] = 0; + out_5271470452060254830[63] = 0; + out_5271470452060254830[64] = 0; + out_5271470452060254830[65] = 0; } -void h_10(double *state, double *unused, double *out_9168195956800795275) { - out_9168195956800795275[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[16] + state[19]; - out_9168195956800795275[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[17] + state[20]; - out_9168195956800795275[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[18] + state[21]; +void h_10(double *state, double *unused, double *out_1276705416788479855) { + out_1276705416788479855[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[16] + state[19]; + out_1276705416788479855[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[17] + state[20]; + out_1276705416788479855[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[18] + state[21]; } -void H_10(double *state, double *unused, double *out_1822418700523668742) { - out_1822418700523668742[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); - out_1822418700523668742[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; - out_1822418700523668742[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2]; - out_1822418700523668742[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; - out_1822418700523668742[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; - out_1822418700523668742[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; - out_1822418700523668742[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; - out_1822418700523668742[7] = 0; - out_1822418700523668742[8] = 0; - out_1822418700523668742[9] = 0; - out_1822418700523668742[10] = 0; - out_1822418700523668742[11] = 0; - out_1822418700523668742[12] = 0; - out_1822418700523668742[13] = 0; - out_1822418700523668742[14] = 0; - out_1822418700523668742[15] = 0; - out_1822418700523668742[16] = 1; - out_1822418700523668742[17] = 0; - out_1822418700523668742[18] = 0; - out_1822418700523668742[19] = 1; - out_1822418700523668742[20] = 0; - out_1822418700523668742[21] = 0; - out_1822418700523668742[22] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; - out_1822418700523668742[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); - out_1822418700523668742[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2]; - out_1822418700523668742[25] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; - out_1822418700523668742[26] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; - out_1822418700523668742[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; - out_1822418700523668742[28] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; - out_1822418700523668742[29] = 0; - out_1822418700523668742[30] = 0; - out_1822418700523668742[31] = 0; - out_1822418700523668742[32] = 0; - out_1822418700523668742[33] = 0; - out_1822418700523668742[34] = 0; - out_1822418700523668742[35] = 0; - out_1822418700523668742[36] = 0; - out_1822418700523668742[37] = 0; - out_1822418700523668742[38] = 0; - out_1822418700523668742[39] = 1; - out_1822418700523668742[40] = 0; - out_1822418700523668742[41] = 0; - out_1822418700523668742[42] = 1; - out_1822418700523668742[43] = 0; - out_1822418700523668742[44] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2]; - out_1822418700523668742[45] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2]; - out_1822418700523668742[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); - out_1822418700523668742[47] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; - out_1822418700523668742[48] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; - out_1822418700523668742[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; - out_1822418700523668742[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; - out_1822418700523668742[51] = 0; - out_1822418700523668742[52] = 0; - out_1822418700523668742[53] = 0; - out_1822418700523668742[54] = 0; - out_1822418700523668742[55] = 0; - out_1822418700523668742[56] = 0; - out_1822418700523668742[57] = 0; - out_1822418700523668742[58] = 0; - out_1822418700523668742[59] = 0; - out_1822418700523668742[60] = 0; - out_1822418700523668742[61] = 0; - out_1822418700523668742[62] = 1; - out_1822418700523668742[63] = 0; - out_1822418700523668742[64] = 0; - out_1822418700523668742[65] = 1; +void H_10(double *state, double *unused, double *out_6589591562994162559) { + out_6589591562994162559[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); + out_6589591562994162559[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; + out_6589591562994162559[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2]; + out_6589591562994162559[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; + out_6589591562994162559[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; + out_6589591562994162559[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; + out_6589591562994162559[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; + out_6589591562994162559[7] = 0; + out_6589591562994162559[8] = 0; + out_6589591562994162559[9] = 0; + out_6589591562994162559[10] = 0; + out_6589591562994162559[11] = 0; + out_6589591562994162559[12] = 0; + out_6589591562994162559[13] = 0; + out_6589591562994162559[14] = 0; + out_6589591562994162559[15] = 0; + out_6589591562994162559[16] = 1; + out_6589591562994162559[17] = 0; + out_6589591562994162559[18] = 0; + out_6589591562994162559[19] = 1; + out_6589591562994162559[20] = 0; + out_6589591562994162559[21] = 0; + out_6589591562994162559[22] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; + out_6589591562994162559[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); + out_6589591562994162559[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2]; + out_6589591562994162559[25] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; + out_6589591562994162559[26] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; + out_6589591562994162559[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; + out_6589591562994162559[28] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; + out_6589591562994162559[29] = 0; + out_6589591562994162559[30] = 0; + out_6589591562994162559[31] = 0; + out_6589591562994162559[32] = 0; + out_6589591562994162559[33] = 0; + out_6589591562994162559[34] = 0; + out_6589591562994162559[35] = 0; + out_6589591562994162559[36] = 0; + out_6589591562994162559[37] = 0; + out_6589591562994162559[38] = 0; + out_6589591562994162559[39] = 1; + out_6589591562994162559[40] = 0; + out_6589591562994162559[41] = 0; + out_6589591562994162559[42] = 1; + out_6589591562994162559[43] = 0; + out_6589591562994162559[44] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2]; + out_6589591562994162559[45] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2]; + out_6589591562994162559[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); + out_6589591562994162559[47] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; + out_6589591562994162559[48] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; + out_6589591562994162559[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; + out_6589591562994162559[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; + out_6589591562994162559[51] = 0; + out_6589591562994162559[52] = 0; + out_6589591562994162559[53] = 0; + out_6589591562994162559[54] = 0; + out_6589591562994162559[55] = 0; + out_6589591562994162559[56] = 0; + out_6589591562994162559[57] = 0; + out_6589591562994162559[58] = 0; + out_6589591562994162559[59] = 0; + out_6589591562994162559[60] = 0; + out_6589591562994162559[61] = 0; + out_6589591562994162559[62] = 1; + out_6589591562994162559[63] = 0; + out_6589591562994162559[64] = 0; + out_6589591562994162559[65] = 1; } -void h_12(double *state, double *unused, double *out_2333080179053030376) { - out_2333080179053030376[0] = state[0]; - out_2333080179053030376[1] = state[1]; - out_2333080179053030376[2] = state[2]; +void h_12(double *state, double *unused, double *out_4489758644873872208) { + out_4489758644873872208[0] = state[0]; + out_4489758644873872208[1] = state[1]; + out_4489758644873872208[2] = state[2]; } -void H_12(double *state, double *unused, double *out_6808063391965579659) { - out_6808063391965579659[0] = 1; - out_6808063391965579659[1] = 0; - out_6808063391965579659[2] = 0; - out_6808063391965579659[3] = 0; - out_6808063391965579659[4] = 0; - out_6808063391965579659[5] = 0; - out_6808063391965579659[6] = 0; - out_6808063391965579659[7] = 0; - out_6808063391965579659[8] = 0; - out_6808063391965579659[9] = 0; - out_6808063391965579659[10] = 0; - out_6808063391965579659[11] = 0; - out_6808063391965579659[12] = 0; - out_6808063391965579659[13] = 0; - out_6808063391965579659[14] = 0; - out_6808063391965579659[15] = 0; - out_6808063391965579659[16] = 0; - out_6808063391965579659[17] = 0; - out_6808063391965579659[18] = 0; - out_6808063391965579659[19] = 0; - out_6808063391965579659[20] = 0; - out_6808063391965579659[21] = 0; - out_6808063391965579659[22] = 0; - out_6808063391965579659[23] = 1; - out_6808063391965579659[24] = 0; - out_6808063391965579659[25] = 0; - out_6808063391965579659[26] = 0; - out_6808063391965579659[27] = 0; - out_6808063391965579659[28] = 0; - out_6808063391965579659[29] = 0; - out_6808063391965579659[30] = 0; - out_6808063391965579659[31] = 0; - out_6808063391965579659[32] = 0; - out_6808063391965579659[33] = 0; - out_6808063391965579659[34] = 0; - out_6808063391965579659[35] = 0; - out_6808063391965579659[36] = 0; - out_6808063391965579659[37] = 0; - out_6808063391965579659[38] = 0; - out_6808063391965579659[39] = 0; - out_6808063391965579659[40] = 0; - out_6808063391965579659[41] = 0; - out_6808063391965579659[42] = 0; - out_6808063391965579659[43] = 0; - out_6808063391965579659[44] = 0; - out_6808063391965579659[45] = 0; - out_6808063391965579659[46] = 1; - out_6808063391965579659[47] = 0; - out_6808063391965579659[48] = 0; - out_6808063391965579659[49] = 0; - out_6808063391965579659[50] = 0; - out_6808063391965579659[51] = 0; - out_6808063391965579659[52] = 0; - out_6808063391965579659[53] = 0; - out_6808063391965579659[54] = 0; - out_6808063391965579659[55] = 0; - out_6808063391965579659[56] = 0; - out_6808063391965579659[57] = 0; - out_6808063391965579659[58] = 0; - out_6808063391965579659[59] = 0; - out_6808063391965579659[60] = 0; - out_6808063391965579659[61] = 0; - out_6808063391965579659[62] = 0; - out_6808063391965579659[63] = 0; - out_6808063391965579659[64] = 0; - out_6808063391965579659[65] = 0; +void H_12(double *state, double *unused, double *out_493203690657883680) { + out_493203690657883680[0] = 1; + out_493203690657883680[1] = 0; + out_493203690657883680[2] = 0; + out_493203690657883680[3] = 0; + out_493203690657883680[4] = 0; + out_493203690657883680[5] = 0; + out_493203690657883680[6] = 0; + out_493203690657883680[7] = 0; + out_493203690657883680[8] = 0; + out_493203690657883680[9] = 0; + out_493203690657883680[10] = 0; + out_493203690657883680[11] = 0; + out_493203690657883680[12] = 0; + out_493203690657883680[13] = 0; + out_493203690657883680[14] = 0; + out_493203690657883680[15] = 0; + out_493203690657883680[16] = 0; + out_493203690657883680[17] = 0; + out_493203690657883680[18] = 0; + out_493203690657883680[19] = 0; + out_493203690657883680[20] = 0; + out_493203690657883680[21] = 0; + out_493203690657883680[22] = 0; + out_493203690657883680[23] = 1; + out_493203690657883680[24] = 0; + out_493203690657883680[25] = 0; + out_493203690657883680[26] = 0; + out_493203690657883680[27] = 0; + out_493203690657883680[28] = 0; + out_493203690657883680[29] = 0; + out_493203690657883680[30] = 0; + out_493203690657883680[31] = 0; + out_493203690657883680[32] = 0; + out_493203690657883680[33] = 0; + out_493203690657883680[34] = 0; + out_493203690657883680[35] = 0; + out_493203690657883680[36] = 0; + out_493203690657883680[37] = 0; + out_493203690657883680[38] = 0; + out_493203690657883680[39] = 0; + out_493203690657883680[40] = 0; + out_493203690657883680[41] = 0; + out_493203690657883680[42] = 0; + out_493203690657883680[43] = 0; + out_493203690657883680[44] = 0; + out_493203690657883680[45] = 0; + out_493203690657883680[46] = 1; + out_493203690657883680[47] = 0; + out_493203690657883680[48] = 0; + out_493203690657883680[49] = 0; + out_493203690657883680[50] = 0; + out_493203690657883680[51] = 0; + out_493203690657883680[52] = 0; + out_493203690657883680[53] = 0; + out_493203690657883680[54] = 0; + out_493203690657883680[55] = 0; + out_493203690657883680[56] = 0; + out_493203690657883680[57] = 0; + out_493203690657883680[58] = 0; + out_493203690657883680[59] = 0; + out_493203690657883680[60] = 0; + out_493203690657883680[61] = 0; + out_493203690657883680[62] = 0; + out_493203690657883680[63] = 0; + out_493203690657883680[64] = 0; + out_493203690657883680[65] = 0; } -void h_35(double *state, double *unused, double *out_4784374292918501119) { - out_4784374292918501119[0] = state[7]; - out_4784374292918501119[1] = state[8]; - out_4784374292918501119[2] = state[9]; +void h_35(double *state, double *unused, double *out_8696122944070004010) { + out_8696122944070004010[0] = state[7]; + out_8696122944070004010[1] = state[8]; + out_8696122944070004010[2] = state[9]; } -void H_35(double *state, double *unused, double *out_8460857742624934078) { - out_8460857742624934078[0] = 0; - out_8460857742624934078[1] = 0; - out_8460857742624934078[2] = 0; - out_8460857742624934078[3] = 0; - out_8460857742624934078[4] = 0; - out_8460857742624934078[5] = 0; - out_8460857742624934078[6] = 0; - out_8460857742624934078[7] = 1; - out_8460857742624934078[8] = 0; - out_8460857742624934078[9] = 0; - out_8460857742624934078[10] = 0; - out_8460857742624934078[11] = 0; - out_8460857742624934078[12] = 0; - out_8460857742624934078[13] = 0; - out_8460857742624934078[14] = 0; - out_8460857742624934078[15] = 0; - out_8460857742624934078[16] = 0; - out_8460857742624934078[17] = 0; - out_8460857742624934078[18] = 0; - out_8460857742624934078[19] = 0; - out_8460857742624934078[20] = 0; - out_8460857742624934078[21] = 0; - out_8460857742624934078[22] = 0; - out_8460857742624934078[23] = 0; - out_8460857742624934078[24] = 0; - out_8460857742624934078[25] = 0; - out_8460857742624934078[26] = 0; - out_8460857742624934078[27] = 0; - out_8460857742624934078[28] = 0; - out_8460857742624934078[29] = 0; - out_8460857742624934078[30] = 1; - out_8460857742624934078[31] = 0; - out_8460857742624934078[32] = 0; - out_8460857742624934078[33] = 0; - out_8460857742624934078[34] = 0; - out_8460857742624934078[35] = 0; - out_8460857742624934078[36] = 0; - out_8460857742624934078[37] = 0; - out_8460857742624934078[38] = 0; - out_8460857742624934078[39] = 0; - out_8460857742624934078[40] = 0; - out_8460857742624934078[41] = 0; - out_8460857742624934078[42] = 0; - out_8460857742624934078[43] = 0; - out_8460857742624934078[44] = 0; - out_8460857742624934078[45] = 0; - out_8460857742624934078[46] = 0; - out_8460857742624934078[47] = 0; - out_8460857742624934078[48] = 0; - out_8460857742624934078[49] = 0; - out_8460857742624934078[50] = 0; - out_8460857742624934078[51] = 0; - out_8460857742624934078[52] = 0; - out_8460857742624934078[53] = 1; - out_8460857742624934078[54] = 0; - out_8460857742624934078[55] = 0; - out_8460857742624934078[56] = 0; - out_8460857742624934078[57] = 0; - out_8460857742624934078[58] = 0; - out_8460857742624934078[59] = 0; - out_8460857742624934078[60] = 0; - out_8460857742624934078[61] = 0; - out_8460857742624934078[62] = 0; - out_8460857742624934078[63] = 0; - out_8460857742624934078[64] = 0; - out_8460857742624934078[65] = 0; +void H_35(double *state, double *unused, double *out_2252359341667130029) { + out_2252359341667130029[0] = 0; + out_2252359341667130029[1] = 0; + out_2252359341667130029[2] = 0; + out_2252359341667130029[3] = 0; + out_2252359341667130029[4] = 0; + out_2252359341667130029[5] = 0; + out_2252359341667130029[6] = 0; + out_2252359341667130029[7] = 1; + out_2252359341667130029[8] = 0; + out_2252359341667130029[9] = 0; + out_2252359341667130029[10] = 0; + out_2252359341667130029[11] = 0; + out_2252359341667130029[12] = 0; + out_2252359341667130029[13] = 0; + out_2252359341667130029[14] = 0; + out_2252359341667130029[15] = 0; + out_2252359341667130029[16] = 0; + out_2252359341667130029[17] = 0; + out_2252359341667130029[18] = 0; + out_2252359341667130029[19] = 0; + out_2252359341667130029[20] = 0; + out_2252359341667130029[21] = 0; + out_2252359341667130029[22] = 0; + out_2252359341667130029[23] = 0; + out_2252359341667130029[24] = 0; + out_2252359341667130029[25] = 0; + out_2252359341667130029[26] = 0; + out_2252359341667130029[27] = 0; + out_2252359341667130029[28] = 0; + out_2252359341667130029[29] = 0; + out_2252359341667130029[30] = 1; + out_2252359341667130029[31] = 0; + out_2252359341667130029[32] = 0; + out_2252359341667130029[33] = 0; + out_2252359341667130029[34] = 0; + out_2252359341667130029[35] = 0; + out_2252359341667130029[36] = 0; + out_2252359341667130029[37] = 0; + out_2252359341667130029[38] = 0; + out_2252359341667130029[39] = 0; + out_2252359341667130029[40] = 0; + out_2252359341667130029[41] = 0; + out_2252359341667130029[42] = 0; + out_2252359341667130029[43] = 0; + out_2252359341667130029[44] = 0; + out_2252359341667130029[45] = 0; + out_2252359341667130029[46] = 0; + out_2252359341667130029[47] = 0; + out_2252359341667130029[48] = 0; + out_2252359341667130029[49] = 0; + out_2252359341667130029[50] = 0; + out_2252359341667130029[51] = 0; + out_2252359341667130029[52] = 0; + out_2252359341667130029[53] = 1; + out_2252359341667130029[54] = 0; + out_2252359341667130029[55] = 0; + out_2252359341667130029[56] = 0; + out_2252359341667130029[57] = 0; + out_2252359341667130029[58] = 0; + out_2252359341667130029[59] = 0; + out_2252359341667130029[60] = 0; + out_2252359341667130029[61] = 0; + out_2252359341667130029[62] = 0; + out_2252359341667130029[63] = 0; + out_2252359341667130029[64] = 0; + out_2252359341667130029[65] = 0; } -void h_32(double *state, double *unused, double *out_2752828006802561656) { - out_2752828006802561656[0] = state[3]; - out_2752828006802561656[1] = state[4]; - out_2752828006802561656[2] = state[5]; - out_2752828006802561656[3] = state[6]; +void h_32(double *state, double *unused, double *out_2592146495080937647) { + out_2592146495080937647[0] = state[3]; + out_2592146495080937647[1] = state[4]; + out_2592146495080937647[2] = state[5]; + out_2592146495080937647[3] = state[6]; } -void H_32(double *state, double *unused, double *out_8329692957684193609) { - out_8329692957684193609[0] = 0; - out_8329692957684193609[1] = 0; - out_8329692957684193609[2] = 0; - out_8329692957684193609[3] = 1; - out_8329692957684193609[4] = 0; - out_8329692957684193609[5] = 0; - out_8329692957684193609[6] = 0; - out_8329692957684193609[7] = 0; - out_8329692957684193609[8] = 0; - out_8329692957684193609[9] = 0; - out_8329692957684193609[10] = 0; - out_8329692957684193609[11] = 0; - out_8329692957684193609[12] = 0; - out_8329692957684193609[13] = 0; - out_8329692957684193609[14] = 0; - out_8329692957684193609[15] = 0; - out_8329692957684193609[16] = 0; - out_8329692957684193609[17] = 0; - out_8329692957684193609[18] = 0; - out_8329692957684193609[19] = 0; - out_8329692957684193609[20] = 0; - out_8329692957684193609[21] = 0; - out_8329692957684193609[22] = 0; - out_8329692957684193609[23] = 0; - out_8329692957684193609[24] = 0; - out_8329692957684193609[25] = 0; - out_8329692957684193609[26] = 1; - out_8329692957684193609[27] = 0; - out_8329692957684193609[28] = 0; - out_8329692957684193609[29] = 0; - out_8329692957684193609[30] = 0; - out_8329692957684193609[31] = 0; - out_8329692957684193609[32] = 0; - out_8329692957684193609[33] = 0; - out_8329692957684193609[34] = 0; - out_8329692957684193609[35] = 0; - out_8329692957684193609[36] = 0; - out_8329692957684193609[37] = 0; - out_8329692957684193609[38] = 0; - out_8329692957684193609[39] = 0; - out_8329692957684193609[40] = 0; - out_8329692957684193609[41] = 0; - out_8329692957684193609[42] = 0; - out_8329692957684193609[43] = 0; - out_8329692957684193609[44] = 0; - out_8329692957684193609[45] = 0; - out_8329692957684193609[46] = 0; - out_8329692957684193609[47] = 0; - out_8329692957684193609[48] = 0; - out_8329692957684193609[49] = 1; - out_8329692957684193609[50] = 0; - out_8329692957684193609[51] = 0; - out_8329692957684193609[52] = 0; - out_8329692957684193609[53] = 0; - out_8329692957684193609[54] = 0; - out_8329692957684193609[55] = 0; - out_8329692957684193609[56] = 0; - out_8329692957684193609[57] = 0; - out_8329692957684193609[58] = 0; - out_8329692957684193609[59] = 0; - out_8329692957684193609[60] = 0; - out_8329692957684193609[61] = 0; - out_8329692957684193609[62] = 0; - out_8329692957684193609[63] = 0; - out_8329692957684193609[64] = 0; - out_8329692957684193609[65] = 0; - out_8329692957684193609[66] = 0; - out_8329692957684193609[67] = 0; - out_8329692957684193609[68] = 0; - out_8329692957684193609[69] = 0; - out_8329692957684193609[70] = 0; - out_8329692957684193609[71] = 0; - out_8329692957684193609[72] = 1; - out_8329692957684193609[73] = 0; - out_8329692957684193609[74] = 0; - out_8329692957684193609[75] = 0; - out_8329692957684193609[76] = 0; - out_8329692957684193609[77] = 0; - out_8329692957684193609[78] = 0; - out_8329692957684193609[79] = 0; - out_8329692957684193609[80] = 0; - out_8329692957684193609[81] = 0; - out_8329692957684193609[82] = 0; - out_8329692957684193609[83] = 0; - out_8329692957684193609[84] = 0; - out_8329692957684193609[85] = 0; - out_8329692957684193609[86] = 0; - out_8329692957684193609[87] = 0; +void H_32(double *state, double *unused, double *out_8522971196901590084) { + out_8522971196901590084[0] = 0; + out_8522971196901590084[1] = 0; + out_8522971196901590084[2] = 0; + out_8522971196901590084[3] = 1; + out_8522971196901590084[4] = 0; + out_8522971196901590084[5] = 0; + out_8522971196901590084[6] = 0; + out_8522971196901590084[7] = 0; + out_8522971196901590084[8] = 0; + out_8522971196901590084[9] = 0; + out_8522971196901590084[10] = 0; + out_8522971196901590084[11] = 0; + out_8522971196901590084[12] = 0; + out_8522971196901590084[13] = 0; + out_8522971196901590084[14] = 0; + out_8522971196901590084[15] = 0; + out_8522971196901590084[16] = 0; + out_8522971196901590084[17] = 0; + out_8522971196901590084[18] = 0; + out_8522971196901590084[19] = 0; + out_8522971196901590084[20] = 0; + out_8522971196901590084[21] = 0; + out_8522971196901590084[22] = 0; + out_8522971196901590084[23] = 0; + out_8522971196901590084[24] = 0; + out_8522971196901590084[25] = 0; + out_8522971196901590084[26] = 1; + out_8522971196901590084[27] = 0; + out_8522971196901590084[28] = 0; + out_8522971196901590084[29] = 0; + out_8522971196901590084[30] = 0; + out_8522971196901590084[31] = 0; + out_8522971196901590084[32] = 0; + out_8522971196901590084[33] = 0; + out_8522971196901590084[34] = 0; + out_8522971196901590084[35] = 0; + out_8522971196901590084[36] = 0; + out_8522971196901590084[37] = 0; + out_8522971196901590084[38] = 0; + out_8522971196901590084[39] = 0; + out_8522971196901590084[40] = 0; + out_8522971196901590084[41] = 0; + out_8522971196901590084[42] = 0; + out_8522971196901590084[43] = 0; + out_8522971196901590084[44] = 0; + out_8522971196901590084[45] = 0; + out_8522971196901590084[46] = 0; + out_8522971196901590084[47] = 0; + out_8522971196901590084[48] = 0; + out_8522971196901590084[49] = 1; + out_8522971196901590084[50] = 0; + out_8522971196901590084[51] = 0; + out_8522971196901590084[52] = 0; + out_8522971196901590084[53] = 0; + out_8522971196901590084[54] = 0; + out_8522971196901590084[55] = 0; + out_8522971196901590084[56] = 0; + out_8522971196901590084[57] = 0; + out_8522971196901590084[58] = 0; + out_8522971196901590084[59] = 0; + out_8522971196901590084[60] = 0; + out_8522971196901590084[61] = 0; + out_8522971196901590084[62] = 0; + out_8522971196901590084[63] = 0; + out_8522971196901590084[64] = 0; + out_8522971196901590084[65] = 0; + out_8522971196901590084[66] = 0; + out_8522971196901590084[67] = 0; + out_8522971196901590084[68] = 0; + out_8522971196901590084[69] = 0; + out_8522971196901590084[70] = 0; + out_8522971196901590084[71] = 0; + out_8522971196901590084[72] = 1; + out_8522971196901590084[73] = 0; + out_8522971196901590084[74] = 0; + out_8522971196901590084[75] = 0; + out_8522971196901590084[76] = 0; + out_8522971196901590084[77] = 0; + out_8522971196901590084[78] = 0; + out_8522971196901590084[79] = 0; + out_8522971196901590084[80] = 0; + out_8522971196901590084[81] = 0; + out_8522971196901590084[82] = 0; + out_8522971196901590084[83] = 0; + out_8522971196901590084[84] = 0; + out_8522971196901590084[85] = 0; + out_8522971196901590084[86] = 0; + out_8522971196901590084[87] = 0; } -void h_13(double *state, double *unused, double *out_392591640858112584) { - out_392591640858112584[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7]; - out_392591640858112584[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8]; - out_392591640858112584[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9]; +void h_13(double *state, double *unused, double *out_5320318911182793356) { + out_5320318911182793356[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7]; + out_5320318911182793356[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8]; + out_5320318911182793356[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9]; } -void H_13(double *state, double *unused, double *out_4626093262240260019) { - out_4626093262240260019[0] = 0; - out_4626093262240260019[1] = 0; - out_4626093262240260019[2] = 0; - out_4626093262240260019[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; - out_4626093262240260019[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; - out_4626093262240260019[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7]; - out_4626093262240260019[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; - out_4626093262240260019[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2); - out_4626093262240260019[8] = 2*state[3]*state[6] + 2*state[4]*state[5]; - out_4626093262240260019[9] = -2*state[3]*state[5] + 2*state[4]*state[6]; - out_4626093262240260019[10] = 0; - out_4626093262240260019[11] = 0; - out_4626093262240260019[12] = 0; - out_4626093262240260019[13] = 0; - out_4626093262240260019[14] = 0; - out_4626093262240260019[15] = 0; - out_4626093262240260019[16] = 0; - out_4626093262240260019[17] = 0; - out_4626093262240260019[18] = 0; - out_4626093262240260019[19] = 0; - out_4626093262240260019[20] = 0; - out_4626093262240260019[21] = 0; - out_4626093262240260019[22] = 0; - out_4626093262240260019[23] = 0; - out_4626093262240260019[24] = 0; - out_4626093262240260019[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; - out_4626093262240260019[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; - out_4626093262240260019[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; - out_4626093262240260019[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8]; - out_4626093262240260019[29] = -2*state[3]*state[6] + 2*state[4]*state[5]; - out_4626093262240260019[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2); - out_4626093262240260019[31] = 2*state[3]*state[4] + 2*state[5]*state[6]; - out_4626093262240260019[32] = 0; - out_4626093262240260019[33] = 0; - out_4626093262240260019[34] = 0; - out_4626093262240260019[35] = 0; - out_4626093262240260019[36] = 0; - out_4626093262240260019[37] = 0; - out_4626093262240260019[38] = 0; - out_4626093262240260019[39] = 0; - out_4626093262240260019[40] = 0; - out_4626093262240260019[41] = 0; - out_4626093262240260019[42] = 0; - out_4626093262240260019[43] = 0; - out_4626093262240260019[44] = 0; - out_4626093262240260019[45] = 0; - out_4626093262240260019[46] = 0; - out_4626093262240260019[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; - out_4626093262240260019[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7]; - out_4626093262240260019[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; - out_4626093262240260019[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; - out_4626093262240260019[51] = 2*state[3]*state[5] + 2*state[4]*state[6]; - out_4626093262240260019[52] = -2*state[3]*state[4] + 2*state[5]*state[6]; - out_4626093262240260019[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2); - out_4626093262240260019[54] = 0; - out_4626093262240260019[55] = 0; - out_4626093262240260019[56] = 0; - out_4626093262240260019[57] = 0; - out_4626093262240260019[58] = 0; - out_4626093262240260019[59] = 0; - out_4626093262240260019[60] = 0; - out_4626093262240260019[61] = 0; - out_4626093262240260019[62] = 0; - out_4626093262240260019[63] = 0; - out_4626093262240260019[64] = 0; - out_4626093262240260019[65] = 0; +void H_13(double *state, double *unused, double *out_6260235854627074248) { + out_6260235854627074248[0] = 0; + out_6260235854627074248[1] = 0; + out_6260235854627074248[2] = 0; + out_6260235854627074248[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; + out_6260235854627074248[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; + out_6260235854627074248[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7]; + out_6260235854627074248[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; + out_6260235854627074248[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2); + out_6260235854627074248[8] = 2*state[3]*state[6] + 2*state[4]*state[5]; + out_6260235854627074248[9] = -2*state[3]*state[5] + 2*state[4]*state[6]; + out_6260235854627074248[10] = 0; + out_6260235854627074248[11] = 0; + out_6260235854627074248[12] = 0; + out_6260235854627074248[13] = 0; + out_6260235854627074248[14] = 0; + out_6260235854627074248[15] = 0; + out_6260235854627074248[16] = 0; + out_6260235854627074248[17] = 0; + out_6260235854627074248[18] = 0; + out_6260235854627074248[19] = 0; + out_6260235854627074248[20] = 0; + out_6260235854627074248[21] = 0; + out_6260235854627074248[22] = 0; + out_6260235854627074248[23] = 0; + out_6260235854627074248[24] = 0; + out_6260235854627074248[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; + out_6260235854627074248[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; + out_6260235854627074248[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; + out_6260235854627074248[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8]; + out_6260235854627074248[29] = -2*state[3]*state[6] + 2*state[4]*state[5]; + out_6260235854627074248[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2); + out_6260235854627074248[31] = 2*state[3]*state[4] + 2*state[5]*state[6]; + out_6260235854627074248[32] = 0; + out_6260235854627074248[33] = 0; + out_6260235854627074248[34] = 0; + out_6260235854627074248[35] = 0; + out_6260235854627074248[36] = 0; + out_6260235854627074248[37] = 0; + out_6260235854627074248[38] = 0; + out_6260235854627074248[39] = 0; + out_6260235854627074248[40] = 0; + out_6260235854627074248[41] = 0; + out_6260235854627074248[42] = 0; + out_6260235854627074248[43] = 0; + out_6260235854627074248[44] = 0; + out_6260235854627074248[45] = 0; + out_6260235854627074248[46] = 0; + out_6260235854627074248[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; + out_6260235854627074248[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7]; + out_6260235854627074248[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; + out_6260235854627074248[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; + out_6260235854627074248[51] = 2*state[3]*state[5] + 2*state[4]*state[6]; + out_6260235854627074248[52] = -2*state[3]*state[4] + 2*state[5]*state[6]; + out_6260235854627074248[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2); + out_6260235854627074248[54] = 0; + out_6260235854627074248[55] = 0; + out_6260235854627074248[56] = 0; + out_6260235854627074248[57] = 0; + out_6260235854627074248[58] = 0; + out_6260235854627074248[59] = 0; + out_6260235854627074248[60] = 0; + out_6260235854627074248[61] = 0; + out_6260235854627074248[62] = 0; + out_6260235854627074248[63] = 0; + out_6260235854627074248[64] = 0; + out_6260235854627074248[65] = 0; } -void h_14(double *state, double *unused, double *out_8367559596262948470) { - out_8367559596262948470[0] = state[10]; - out_8367559596262948470[1] = state[11]; - out_8367559596262948470[2] = state[12]; +void h_14(double *state, double *unused, double *out_4264504783842197092) { + out_4264504783842197092[0] = state[10]; + out_4264504783842197092[1] = state[11]; + out_4264504783842197092[2] = state[12]; } -void H_14(double *state, double *unused, double *out_6860413920341600807) { - out_6860413920341600807[0] = 0; - out_6860413920341600807[1] = 0; - out_6860413920341600807[2] = 0; - out_6860413920341600807[3] = 0; - out_6860413920341600807[4] = 0; - out_6860413920341600807[5] = 0; - out_6860413920341600807[6] = 0; - out_6860413920341600807[7] = 0; - out_6860413920341600807[8] = 0; - out_6860413920341600807[9] = 0; - out_6860413920341600807[10] = 1; - out_6860413920341600807[11] = 0; - out_6860413920341600807[12] = 0; - out_6860413920341600807[13] = 0; - out_6860413920341600807[14] = 0; - out_6860413920341600807[15] = 0; - out_6860413920341600807[16] = 0; - out_6860413920341600807[17] = 0; - out_6860413920341600807[18] = 0; - out_6860413920341600807[19] = 0; - out_6860413920341600807[20] = 0; - out_6860413920341600807[21] = 0; - out_6860413920341600807[22] = 0; - out_6860413920341600807[23] = 0; - out_6860413920341600807[24] = 0; - out_6860413920341600807[25] = 0; - out_6860413920341600807[26] = 0; - out_6860413920341600807[27] = 0; - out_6860413920341600807[28] = 0; - out_6860413920341600807[29] = 0; - out_6860413920341600807[30] = 0; - out_6860413920341600807[31] = 0; - out_6860413920341600807[32] = 0; - out_6860413920341600807[33] = 1; - out_6860413920341600807[34] = 0; - out_6860413920341600807[35] = 0; - out_6860413920341600807[36] = 0; - out_6860413920341600807[37] = 0; - out_6860413920341600807[38] = 0; - out_6860413920341600807[39] = 0; - out_6860413920341600807[40] = 0; - out_6860413920341600807[41] = 0; - out_6860413920341600807[42] = 0; - out_6860413920341600807[43] = 0; - out_6860413920341600807[44] = 0; - out_6860413920341600807[45] = 0; - out_6860413920341600807[46] = 0; - out_6860413920341600807[47] = 0; - out_6860413920341600807[48] = 0; - out_6860413920341600807[49] = 0; - out_6860413920341600807[50] = 0; - out_6860413920341600807[51] = 0; - out_6860413920341600807[52] = 0; - out_6860413920341600807[53] = 0; - out_6860413920341600807[54] = 0; - out_6860413920341600807[55] = 0; - out_6860413920341600807[56] = 1; - out_6860413920341600807[57] = 0; - out_6860413920341600807[58] = 0; - out_6860413920341600807[59] = 0; - out_6860413920341600807[60] = 0; - out_6860413920341600807[61] = 0; - out_6860413920341600807[62] = 0; - out_6860413920341600807[63] = 0; - out_6860413920341600807[64] = 0; - out_6860413920341600807[65] = 0; +void H_14(double *state, double *unused, double *out_5271470452060254830) { + out_5271470452060254830[0] = 0; + out_5271470452060254830[1] = 0; + out_5271470452060254830[2] = 0; + out_5271470452060254830[3] = 0; + out_5271470452060254830[4] = 0; + out_5271470452060254830[5] = 0; + out_5271470452060254830[6] = 0; + out_5271470452060254830[7] = 0; + out_5271470452060254830[8] = 0; + out_5271470452060254830[9] = 0; + out_5271470452060254830[10] = 1; + out_5271470452060254830[11] = 0; + out_5271470452060254830[12] = 0; + out_5271470452060254830[13] = 0; + out_5271470452060254830[14] = 0; + out_5271470452060254830[15] = 0; + out_5271470452060254830[16] = 0; + out_5271470452060254830[17] = 0; + out_5271470452060254830[18] = 0; + out_5271470452060254830[19] = 0; + out_5271470452060254830[20] = 0; + out_5271470452060254830[21] = 0; + out_5271470452060254830[22] = 0; + out_5271470452060254830[23] = 0; + out_5271470452060254830[24] = 0; + out_5271470452060254830[25] = 0; + out_5271470452060254830[26] = 0; + out_5271470452060254830[27] = 0; + out_5271470452060254830[28] = 0; + out_5271470452060254830[29] = 0; + out_5271470452060254830[30] = 0; + out_5271470452060254830[31] = 0; + out_5271470452060254830[32] = 0; + out_5271470452060254830[33] = 1; + out_5271470452060254830[34] = 0; + out_5271470452060254830[35] = 0; + out_5271470452060254830[36] = 0; + out_5271470452060254830[37] = 0; + out_5271470452060254830[38] = 0; + out_5271470452060254830[39] = 0; + out_5271470452060254830[40] = 0; + out_5271470452060254830[41] = 0; + out_5271470452060254830[42] = 0; + out_5271470452060254830[43] = 0; + out_5271470452060254830[44] = 0; + out_5271470452060254830[45] = 0; + out_5271470452060254830[46] = 0; + out_5271470452060254830[47] = 0; + out_5271470452060254830[48] = 0; + out_5271470452060254830[49] = 0; + out_5271470452060254830[50] = 0; + out_5271470452060254830[51] = 0; + out_5271470452060254830[52] = 0; + out_5271470452060254830[53] = 0; + out_5271470452060254830[54] = 0; + out_5271470452060254830[55] = 0; + out_5271470452060254830[56] = 1; + out_5271470452060254830[57] = 0; + out_5271470452060254830[58] = 0; + out_5271470452060254830[59] = 0; + out_5271470452060254830[60] = 0; + out_5271470452060254830[61] = 0; + out_5271470452060254830[62] = 0; + out_5271470452060254830[63] = 0; + out_5271470452060254830[64] = 0; + out_5271470452060254830[65] = 0; } -void h_33(double *state, double *unused, double *out_9004392775721978689) { - out_9004392775721978689[0] = state[16]; - out_9004392775721978689[1] = state[17]; - out_9004392775721978689[2] = state[18]; +void h_33(double *state, double *unused, double *out_57093419937807770) { + out_57093419937807770[0] = state[16]; + out_57093419937807770[1] = state[17]; + out_57093419937807770[2] = state[18]; } -void H_33(double *state, double *unused, double *out_5310300737986076474) { - out_5310300737986076474[0] = 0; - out_5310300737986076474[1] = 0; - out_5310300737986076474[2] = 0; - out_5310300737986076474[3] = 0; - out_5310300737986076474[4] = 0; - out_5310300737986076474[5] = 0; - out_5310300737986076474[6] = 0; - out_5310300737986076474[7] = 0; - out_5310300737986076474[8] = 0; - out_5310300737986076474[9] = 0; - out_5310300737986076474[10] = 0; - out_5310300737986076474[11] = 0; - out_5310300737986076474[12] = 0; - out_5310300737986076474[13] = 0; - out_5310300737986076474[14] = 0; - out_5310300737986076474[15] = 0; - out_5310300737986076474[16] = 1; - out_5310300737986076474[17] = 0; - out_5310300737986076474[18] = 0; - out_5310300737986076474[19] = 0; - out_5310300737986076474[20] = 0; - out_5310300737986076474[21] = 0; - out_5310300737986076474[22] = 0; - out_5310300737986076474[23] = 0; - out_5310300737986076474[24] = 0; - out_5310300737986076474[25] = 0; - out_5310300737986076474[26] = 0; - out_5310300737986076474[27] = 0; - out_5310300737986076474[28] = 0; - out_5310300737986076474[29] = 0; - out_5310300737986076474[30] = 0; - out_5310300737986076474[31] = 0; - out_5310300737986076474[32] = 0; - out_5310300737986076474[33] = 0; - out_5310300737986076474[34] = 0; - out_5310300737986076474[35] = 0; - out_5310300737986076474[36] = 0; - out_5310300737986076474[37] = 0; - out_5310300737986076474[38] = 0; - out_5310300737986076474[39] = 1; - out_5310300737986076474[40] = 0; - out_5310300737986076474[41] = 0; - out_5310300737986076474[42] = 0; - out_5310300737986076474[43] = 0; - out_5310300737986076474[44] = 0; - out_5310300737986076474[45] = 0; - out_5310300737986076474[46] = 0; - out_5310300737986076474[47] = 0; - out_5310300737986076474[48] = 0; - out_5310300737986076474[49] = 0; - out_5310300737986076474[50] = 0; - out_5310300737986076474[51] = 0; - out_5310300737986076474[52] = 0; - out_5310300737986076474[53] = 0; - out_5310300737986076474[54] = 0; - out_5310300737986076474[55] = 0; - out_5310300737986076474[56] = 0; - out_5310300737986076474[57] = 0; - out_5310300737986076474[58] = 0; - out_5310300737986076474[59] = 0; - out_5310300737986076474[60] = 0; - out_5310300737986076474[61] = 0; - out_5310300737986076474[62] = 1; - out_5310300737986076474[63] = 0; - out_5310300737986076474[64] = 0; - out_5310300737986076474[65] = 0; +void H_33(double *state, double *unused, double *out_5402916346305987633) { + out_5402916346305987633[0] = 0; + out_5402916346305987633[1] = 0; + out_5402916346305987633[2] = 0; + out_5402916346305987633[3] = 0; + out_5402916346305987633[4] = 0; + out_5402916346305987633[5] = 0; + out_5402916346305987633[6] = 0; + out_5402916346305987633[7] = 0; + out_5402916346305987633[8] = 0; + out_5402916346305987633[9] = 0; + out_5402916346305987633[10] = 0; + out_5402916346305987633[11] = 0; + out_5402916346305987633[12] = 0; + out_5402916346305987633[13] = 0; + out_5402916346305987633[14] = 0; + out_5402916346305987633[15] = 0; + out_5402916346305987633[16] = 1; + out_5402916346305987633[17] = 0; + out_5402916346305987633[18] = 0; + out_5402916346305987633[19] = 0; + out_5402916346305987633[20] = 0; + out_5402916346305987633[21] = 0; + out_5402916346305987633[22] = 0; + out_5402916346305987633[23] = 0; + out_5402916346305987633[24] = 0; + out_5402916346305987633[25] = 0; + out_5402916346305987633[26] = 0; + out_5402916346305987633[27] = 0; + out_5402916346305987633[28] = 0; + out_5402916346305987633[29] = 0; + out_5402916346305987633[30] = 0; + out_5402916346305987633[31] = 0; + out_5402916346305987633[32] = 0; + out_5402916346305987633[33] = 0; + out_5402916346305987633[34] = 0; + out_5402916346305987633[35] = 0; + out_5402916346305987633[36] = 0; + out_5402916346305987633[37] = 0; + out_5402916346305987633[38] = 0; + out_5402916346305987633[39] = 1; + out_5402916346305987633[40] = 0; + out_5402916346305987633[41] = 0; + out_5402916346305987633[42] = 0; + out_5402916346305987633[43] = 0; + out_5402916346305987633[44] = 0; + out_5402916346305987633[45] = 0; + out_5402916346305987633[46] = 0; + out_5402916346305987633[47] = 0; + out_5402916346305987633[48] = 0; + out_5402916346305987633[49] = 0; + out_5402916346305987633[50] = 0; + out_5402916346305987633[51] = 0; + out_5402916346305987633[52] = 0; + out_5402916346305987633[53] = 0; + out_5402916346305987633[54] = 0; + out_5402916346305987633[55] = 0; + out_5402916346305987633[56] = 0; + out_5402916346305987633[57] = 0; + out_5402916346305987633[58] = 0; + out_5402916346305987633[59] = 0; + out_5402916346305987633[60] = 0; + out_5402916346305987633[61] = 0; + out_5402916346305987633[62] = 1; + out_5402916346305987633[63] = 0; + out_5402916346305987633[64] = 0; + out_5402916346305987633[65] = 0; } #include #include @@ -1855,77 +1855,77 @@ void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, doub void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_33, H_33, NULL, in_z, in_R, in_ea, MAHA_THRESH_33); } -void live_H(double *in_vec, double *out_3693999405139314794) { - H(in_vec, out_3693999405139314794); +void live_H(double *in_vec, double *out_8687680363762493539) { + H(in_vec, out_8687680363762493539); } -void live_err_fun(double *nom_x, double *delta_x, double *out_3414598820419176679) { - err_fun(nom_x, delta_x, out_3414598820419176679); +void live_err_fun(double *nom_x, double *delta_x, double *out_8136490426412747881) { + err_fun(nom_x, delta_x, out_8136490426412747881); } -void live_inv_err_fun(double *nom_x, double *true_x, double *out_5364111469669926070) { - inv_err_fun(nom_x, true_x, out_5364111469669926070); +void live_inv_err_fun(double *nom_x, double *true_x, double *out_1524655933590429885) { + inv_err_fun(nom_x, true_x, out_1524655933590429885); } -void live_H_mod_fun(double *state, double *out_8664355600254952032) { - H_mod_fun(state, out_8664355600254952032); +void live_H_mod_fun(double *state, double *out_7453067886472422609) { + H_mod_fun(state, out_7453067886472422609); } -void live_f_fun(double *state, double dt, double *out_5944879704050977948) { - f_fun(state, dt, out_5944879704050977948); +void live_f_fun(double *state, double dt, double *out_3547963935686363060) { + f_fun(state, dt, out_3547963935686363060); } -void live_F_fun(double *state, double dt, double *out_1511312995606333672) { - F_fun(state, dt, out_1511312995606333672); +void live_F_fun(double *state, double dt, double *out_3254137990445528781) { + F_fun(state, dt, out_3254137990445528781); } -void live_h_4(double *state, double *unused, double *out_6748587584352144008) { - h_4(state, unused, out_6748587584352144008); +void live_h_4(double *state, double *unused, double *out_4410787759120580449) { + h_4(state, unused, out_4410787759120580449); } -void live_H_4(double *state, double *unused, double *out_6619224273712010162) { - H_4(state, unused, out_6619224273712010162); +void live_H_4(double *state, double *unused, double *out_5512660098689845475) { + H_4(state, unused, out_5512660098689845475); } -void live_h_9(double *state, double *unused, double *out_8367559596262948470) { - h_9(state, unused, out_8367559596262948470); +void live_h_9(double *state, double *unused, double *out_4264504783842197092) { + h_9(state, unused, out_4264504783842197092); } -void live_H_9(double *state, double *unused, double *out_6860413920341600807) { - H_9(state, unused, out_6860413920341600807); +void live_H_9(double *state, double *unused, double *out_5271470452060254830) { + H_9(state, unused, out_5271470452060254830); } -void live_h_10(double *state, double *unused, double *out_9168195956800795275) { - h_10(state, unused, out_9168195956800795275); +void live_h_10(double *state, double *unused, double *out_1276705416788479855) { + h_10(state, unused, out_1276705416788479855); } -void live_H_10(double *state, double *unused, double *out_1822418700523668742) { - H_10(state, unused, out_1822418700523668742); +void live_H_10(double *state, double *unused, double *out_6589591562994162559) { + H_10(state, unused, out_6589591562994162559); } -void live_h_12(double *state, double *unused, double *out_2333080179053030376) { - h_12(state, unused, out_2333080179053030376); +void live_h_12(double *state, double *unused, double *out_4489758644873872208) { + h_12(state, unused, out_4489758644873872208); } -void live_H_12(double *state, double *unused, double *out_6808063391965579659) { - H_12(state, unused, out_6808063391965579659); +void live_H_12(double *state, double *unused, double *out_493203690657883680) { + H_12(state, unused, out_493203690657883680); } -void live_h_35(double *state, double *unused, double *out_4784374292918501119) { - h_35(state, unused, out_4784374292918501119); +void live_h_35(double *state, double *unused, double *out_8696122944070004010) { + h_35(state, unused, out_8696122944070004010); } -void live_H_35(double *state, double *unused, double *out_8460857742624934078) { - H_35(state, unused, out_8460857742624934078); +void live_H_35(double *state, double *unused, double *out_2252359341667130029) { + H_35(state, unused, out_2252359341667130029); } -void live_h_32(double *state, double *unused, double *out_2752828006802561656) { - h_32(state, unused, out_2752828006802561656); +void live_h_32(double *state, double *unused, double *out_2592146495080937647) { + h_32(state, unused, out_2592146495080937647); } -void live_H_32(double *state, double *unused, double *out_8329692957684193609) { - H_32(state, unused, out_8329692957684193609); +void live_H_32(double *state, double *unused, double *out_8522971196901590084) { + H_32(state, unused, out_8522971196901590084); } -void live_h_13(double *state, double *unused, double *out_392591640858112584) { - h_13(state, unused, out_392591640858112584); +void live_h_13(double *state, double *unused, double *out_5320318911182793356) { + h_13(state, unused, out_5320318911182793356); } -void live_H_13(double *state, double *unused, double *out_4626093262240260019) { - H_13(state, unused, out_4626093262240260019); +void live_H_13(double *state, double *unused, double *out_6260235854627074248) { + H_13(state, unused, out_6260235854627074248); } -void live_h_14(double *state, double *unused, double *out_8367559596262948470) { - h_14(state, unused, out_8367559596262948470); +void live_h_14(double *state, double *unused, double *out_4264504783842197092) { + h_14(state, unused, out_4264504783842197092); } -void live_H_14(double *state, double *unused, double *out_6860413920341600807) { - H_14(state, unused, out_6860413920341600807); +void live_H_14(double *state, double *unused, double *out_5271470452060254830) { + H_14(state, unused, out_5271470452060254830); } -void live_h_33(double *state, double *unused, double *out_9004392775721978689) { - h_33(state, unused, out_9004392775721978689); +void live_h_33(double *state, double *unused, double *out_57093419937807770) { + h_33(state, unused, out_57093419937807770); } -void live_H_33(double *state, double *unused, double *out_5310300737986076474) { - H_33(state, unused, out_5310300737986076474); +void live_H_33(double *state, double *unused, double *out_5402916346305987633) { + H_33(state, unused, out_5402916346305987633); } void live_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/live.h b/selfdrive/locationd/models/generated/live.h index 58aa13cf8..505753497 100644 --- a/selfdrive/locationd/models/generated/live.h +++ b/selfdrive/locationd/models/generated/live.h @@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void live_H(double *in_vec, double *out_3693999405139314794); -void live_err_fun(double *nom_x, double *delta_x, double *out_3414598820419176679); -void live_inv_err_fun(double *nom_x, double *true_x, double *out_5364111469669926070); -void live_H_mod_fun(double *state, double *out_8664355600254952032); -void live_f_fun(double *state, double dt, double *out_5944879704050977948); -void live_F_fun(double *state, double dt, double *out_1511312995606333672); -void live_h_4(double *state, double *unused, double *out_6748587584352144008); -void live_H_4(double *state, double *unused, double *out_6619224273712010162); -void live_h_9(double *state, double *unused, double *out_8367559596262948470); -void live_H_9(double *state, double *unused, double *out_6860413920341600807); -void live_h_10(double *state, double *unused, double *out_9168195956800795275); -void live_H_10(double *state, double *unused, double *out_1822418700523668742); -void live_h_12(double *state, double *unused, double *out_2333080179053030376); -void live_H_12(double *state, double *unused, double *out_6808063391965579659); -void live_h_35(double *state, double *unused, double *out_4784374292918501119); -void live_H_35(double *state, double *unused, double *out_8460857742624934078); -void live_h_32(double *state, double *unused, double *out_2752828006802561656); -void live_H_32(double *state, double *unused, double *out_8329692957684193609); -void live_h_13(double *state, double *unused, double *out_392591640858112584); -void live_H_13(double *state, double *unused, double *out_4626093262240260019); -void live_h_14(double *state, double *unused, double *out_8367559596262948470); -void live_H_14(double *state, double *unused, double *out_6860413920341600807); -void live_h_33(double *state, double *unused, double *out_9004392775721978689); -void live_H_33(double *state, double *unused, double *out_5310300737986076474); +void live_H(double *in_vec, double *out_8687680363762493539); +void live_err_fun(double *nom_x, double *delta_x, double *out_8136490426412747881); +void live_inv_err_fun(double *nom_x, double *true_x, double *out_1524655933590429885); +void live_H_mod_fun(double *state, double *out_7453067886472422609); +void live_f_fun(double *state, double dt, double *out_3547963935686363060); +void live_F_fun(double *state, double dt, double *out_3254137990445528781); +void live_h_4(double *state, double *unused, double *out_4410787759120580449); +void live_H_4(double *state, double *unused, double *out_5512660098689845475); +void live_h_9(double *state, double *unused, double *out_4264504783842197092); +void live_H_9(double *state, double *unused, double *out_5271470452060254830); +void live_h_10(double *state, double *unused, double *out_1276705416788479855); +void live_H_10(double *state, double *unused, double *out_6589591562994162559); +void live_h_12(double *state, double *unused, double *out_4489758644873872208); +void live_H_12(double *state, double *unused, double *out_493203690657883680); +void live_h_35(double *state, double *unused, double *out_8696122944070004010); +void live_H_35(double *state, double *unused, double *out_2252359341667130029); +void live_h_32(double *state, double *unused, double *out_2592146495080937647); +void live_H_32(double *state, double *unused, double *out_8522971196901590084); +void live_h_13(double *state, double *unused, double *out_5320318911182793356); +void live_H_13(double *state, double *unused, double *out_6260235854627074248); +void live_h_14(double *state, double *unused, double *out_4264504783842197092); +void live_H_14(double *state, double *unused, double *out_5271470452060254830); +void live_h_33(double *state, double *unused, double *out_57093419937807770); +void live_H_33(double *state, double *unused, double *out_5402916346305987633); void live_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/locationd/ubloxd b/selfdrive/locationd/ubloxd index 2a92fafb0..0855f7ca6 100755 Binary files a/selfdrive/locationd/ubloxd and b/selfdrive/locationd/ubloxd differ diff --git a/selfdrive/loggerd/config.py b/selfdrive/loggerd/config.py index f246b1ea4..33f3a8caf 100644 --- a/selfdrive/loggerd/config.py +++ b/selfdrive/loggerd/config.py @@ -8,11 +8,8 @@ if os.environ.get('LOG_ROOT', False): elif PC: ROOT = os.path.join(str(Path.home()), ".comma", "media", "0", "realdata") else: - params = Params() - if int(params.get('dp_atl', encoding='utf8')) > 0 or params.get_bool('dp_jetson') or params.get_bool('dp_api_custom'): - ROOT = '/data/media/0/fakedata/' - else: - ROOT = '/data/media/0/realdata/' + ROOT = '/data/media/0/fakedata/' + #ROOT = '/data/media/0/realdata/' CAMERA_FPS = 20 diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 421fd7367..fb53cb64b 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -26,6 +26,7 @@ procs = [ # NativeProcess("logcatd", "system/logcatd", ["./logcatd"]), # NativeProcess("proclogd", "system/proclogd", ["./proclogd"]), # PythonProcess("logmessaged", "system.logmessaged", offroad=True), + # PythonProcess("micd", "system.micd"), # PythonProcess("timezoned", "system.timezoned", enabled=not PC, offroad=True), DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), @@ -33,6 +34,8 @@ procs = [ # NativeProcess("encoderd", "selfdrive/loggerd", ["./encoderd"]), # NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"], onroad=False, callback=logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]), + NativeProcess("mapsd", "selfdrive/navd", ["./map_renderer"], enabled=False), + # NativeProcess("navmodeld", "selfdrive/modeld", ["./navmodeld"], enabled=False), NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC, offroad=EON, sigkill=EON), NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)), NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if (TICI or EON) else None)), diff --git a/selfdrive/mapd/lib/osm.py b/selfdrive/mapd/lib/osm.py index 32c01110d..25c73d7ea 100644 --- a/selfdrive/mapd/lib/osm.py +++ b/selfdrive/mapd/lib/osm.py @@ -17,7 +17,7 @@ class OSM(): def __init__(self): self.api = overpy.Overpass() self.areas = None - self.osm_local_db_enabled = is_local_osm_installed() and Params().get_bool("OsmLocalDb") + self.osm_local_db_enabled = is_local_osm_installed() # self.api = overpy.Overpass(url='http://3.65.170.21/api/interpreter') def fetch_road_ways_around_location(self, lat, lon, radius): diff --git a/selfdrive/modeld/_dmonitoringmodeld b/selfdrive/modeld/_dmonitoringmodeld index 7b9c3f151..6a3e62098 100755 Binary files a/selfdrive/modeld/_dmonitoringmodeld and b/selfdrive/modeld/_dmonitoringmodeld differ diff --git a/selfdrive/modeld/_modeld b/selfdrive/modeld/_modeld index 006bcc73f..7bcbe9c80 100755 Binary files a/selfdrive/modeld/_modeld and b/selfdrive/modeld/_modeld differ diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 059a85e7d..044d75a63 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -13,6 +13,7 @@ #endif #include "common/mat.h" +#include "cereal/messaging/messaging.h" #include "selfdrive/modeld/transforms/loadyuv.h" #include "selfdrive/modeld/transforms/transform.h" @@ -21,6 +22,11 @@ const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL; void softmax(const float* input, float* output, size_t len); float sigmoid(float input); +template +constexpr const kj::ArrayPtr to_kj_array_ptr(const std::array &arr) { + return kj::ArrayPtr(arr.data(), arr.size()); +} + class ModelFrame { public: ModelFrame(cl_device_id device_id, cl_context context); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index e2ee812e4..92a9b1d4f 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -14,11 +14,13 @@ #include "common/modeldata.h" #include "common/util.h" #include "selfdrive/modeld/models/commonmodel.h" +#include "selfdrive/modeld/models/nav.h" #include "selfdrive/modeld/runners/run.h" constexpr int DESIRE_LEN = 8; constexpr int DESIRE_PRED_LEN = 4; constexpr int TRAFFIC_CONVENTION_LEN = 2; +constexpr int DRIVING_STYLE_LEN = 12; constexpr int MODEL_FREQ = 20; constexpr int DISENGAGE_LEN = 5; @@ -264,11 +266,17 @@ struct ModelState { #ifdef TRAFFIC_CONVENTION float traffic_convention[TRAFFIC_CONVENTION_LEN] = {}; #endif +#ifdef DRIVING_STYLE + float driving_style[DRIVING_STYLE_LEN] = {}; +#endif +#ifdef NAV + float nav_features[NAV_FEATURE_LEN] = {}; +#endif }; void model_init(ModelState* s, cl_device_id device_id, cl_context context); ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide, - const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only); + const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only); void model_free(ModelState* s); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, const ModelOutput &net_outputs, uint64_t timestamp_eof, diff --git a/selfdrive/modeld/models/nav.h b/selfdrive/modeld/models/nav.h new file mode 100644 index 000000000..b2955ad2c --- /dev/null +++ b/selfdrive/modeld/models/nav.h @@ -0,0 +1,56 @@ +#pragma once + +#include "cereal/messaging/messaging.h" +#include "cereal/visionipc/visionipc_client.h" +#include "common/util.h" +#include "common/modeldata.h" +#include "selfdrive/modeld/models/commonmodel.h" +#include "selfdrive/modeld/runners/run.h" + +constexpr int NAV_INPUT_SIZE = 256*256; +constexpr int NAV_FEATURE_LEN = 64; +constexpr int NAV_DESIRE_LEN = 32; + +struct NavModelOutputXY { + float x; + float y; +}; +static_assert(sizeof(NavModelOutputXY) == sizeof(float)*2); + +struct NavModelOutputPlan { + std::array mean; + std::array std; +}; +static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2); + +struct NavModelOutputDesirePrediction { + std::array values; +}; +static_assert(sizeof(NavModelOutputDesirePrediction) == sizeof(float)*NAV_DESIRE_LEN); + +struct NavModelOutputFeatures { + std::array values; +}; +static_assert(sizeof(NavModelOutputFeatures) == sizeof(float)*NAV_FEATURE_LEN); + +struct NavModelResult { + const NavModelOutputPlan plan; + const NavModelOutputDesirePrediction desire_pred; + const NavModelOutputFeatures features; + float dsp_execution_time; +}; +static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlan) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float)); + +constexpr int NAV_OUTPUT_SIZE = sizeof(NavModelResult) / sizeof(float); +constexpr int NAV_NET_OUTPUT_SIZE = NAV_OUTPUT_SIZE - 1; + +struct NavModelState { + RunModel *m; + uint8_t net_input_buf[NAV_INPUT_SIZE]; + float output[NAV_OUTPUT_SIZE]; +}; + +void navmodel_init(NavModelState* s); +NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf); +void navmodel_publish(PubMaster &pm, uint32_t frame_id, const NavModelResult &model_res, float execution_time); +void navmodel_free(NavModelState* s); diff --git a/selfdrive/modeld/models/navmodel_q.dlc b/selfdrive/modeld/models/navmodel_q.dlc new file mode 100644 index 000000000..21d40c2cd Binary files /dev/null and b/selfdrive/modeld/models/navmodel_q.dlc differ diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx new file mode 100644 index 000000000..749ac1287 Binary files /dev/null and b/selfdrive/modeld/models/supercombo.onnx differ diff --git a/selfdrive/modeld/models/supercombo.thneed b/selfdrive/modeld/models/supercombo.thneed index 04fe8328e..e5b4db3b4 100644 Binary files a/selfdrive/modeld/models/supercombo.thneed and b/selfdrive/modeld/models/supercombo.thneed differ diff --git a/selfdrive/modeld/navmodeld b/selfdrive/modeld/navmodeld new file mode 100755 index 000000000..079afd967 --- /dev/null +++ b/selfdrive/modeld/navmodeld @@ -0,0 +1,12 @@ +#!/bin/sh + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +cd $DIR + +if [ -f /TICI ]; then + export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH" + export ADSP_LIBRARY_PATH="/data/pythonpath/third_party/snpe/dsp/" +else + export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH" +fi +exec ./_navmodeld diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h index 02b8c4b20..487a2a1fe 100644 --- a/selfdrive/modeld/runners/runmodel.h +++ b/selfdrive/modeld/runners/runmodel.h @@ -4,6 +4,8 @@ public: virtual ~RunModel() {} virtual void addRecurrent(float *state, int state_size) {} virtual void addDesire(float *state, int state_size) {} + virtual void addNavFeatures(float *state, int state_size) {} + virtual void addDrivingStyle(float *state, int state_size) {} virtual void addTrafficConvention(float *state, int state_size) {} virtual void addCalib(float *state, int state_size) {} virtual void addImage(float *image_buf, int buf_size) {} diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index 584c63680..a5823c5da 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -27,6 +27,8 @@ public: void addTrafficConvention(float *state, int state_size); void addCalib(float *state, int state_size); void addDesire(float *state, int state_size); + void addDrivingStyle(float *state, int state_size); + void addNavFeatures(float *state, int state_size); void addImage(float *image_buf, int buf_size); void addExtra(float *image_buf, int buf_size); void execute(); @@ -72,6 +74,10 @@ private: std::unique_ptr trafficConventionBuffer; float *desire; std::unique_ptr desireBuffer; + float *navFeatures; + std::unique_ptr navFeaturesBuffer; + float *drivingStyle; + std::unique_ptr drivingStyleBuffer; float *calib; std::unique_ptr calibBuffer; }; diff --git a/selfdrive/modeld/runners/thneedmodel.h b/selfdrive/modeld/runners/thneedmodel.h index fe0f9ae44..447bc7bbc 100644 --- a/selfdrive/modeld/runners/thneedmodel.h +++ b/selfdrive/modeld/runners/thneedmodel.h @@ -9,6 +9,8 @@ public: void addRecurrent(float *state, int state_size); void addTrafficConvention(float *state, int state_size); void addDesire(float *state, int state_size); + void addNavFeatures(float *state, int state_size); + void addDrivingStyle(float *state, int state_size); void addImage(float *image_buf, int buf_size); void addExtra(float *image_buf, int buf_size); void execute(); @@ -26,6 +28,8 @@ private: // recurrent and desire float *recurrent; float *trafficConvention; + float *drivingStyle; float *desire; + float *navFeatures; }; diff --git a/selfdrive/modeld/thneed/compile b/selfdrive/modeld/thneed/compile index d790ec213..3935684aa 100755 Binary files a/selfdrive/modeld/thneed/compile and b/selfdrive/modeld/thneed/compile differ diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 4fd703ec1..ee4dd52e0 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -150,6 +150,11 @@ class DriverStatus(): self._set_timers(active_monitoring=True) + def _reset_awareness(self): + self.awareness = 1. + self.awareness_active = 1. + self.awareness_passive = 1. + def _set_timers(self, active_monitoring): if self.active_monitoring_mode and self.awareness <= self.threshold_prompt: if active_monitoring: @@ -276,17 +281,17 @@ class DriverStatus(): self.hi_stds = 0 def update_events(self, events, driver_engaged, ctrl_active, standstill): - if (driver_engaged and self.awareness > 0) or not ctrl_active: - # reset only when on disengagement if red reached - self.awareness = 1. - self.awareness_active = 1. - self.awareness_passive = 1. + if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or not ctrl_active: # reset only when on disengagement if red reached + self._reset_awareness() return driver_attentive = self.driver_distraction_filter.x < 0.37 awareness_prev = self.awareness if (driver_attentive and self.face_detected and self.pose.low_std and self.awareness > 0): + if driver_engaged: + self._reset_awareness() + return # only restore awareness when paying attention and alert is not red self.awareness = min(self.awareness + ((self.settings._RECOVERY_FACTOR_MAX-self.settings._RECOVERY_FACTOR_MIN)*(1.-self.awareness)+self.settings._RECOVERY_FACTOR_MIN)*self.step_change, 1.) if self.awareness == 1.: diff --git a/selfdrive/sensord/_sensord b/selfdrive/sensord/_sensord index f58c95aa1..49a259486 100755 Binary files a/selfdrive/sensord/_sensord and b/selfdrive/sensord/_sensord differ diff --git a/selfdrive/sensord/sensors/i2c_sensor.h b/selfdrive/sensord/sensors/i2c_sensor.h index 0de2a9873..08ca6f09c 100644 --- a/selfdrive/sensord/sensors/i2c_sensor.h +++ b/selfdrive/sensord/sensors/i2c_sensor.h @@ -2,12 +2,12 @@ #include #include - #include "cereal/gen/cpp/log.capnp.h" #include "common/i2c.h" #include "common/gpio.h" +#include "common/swaglog.h" #include "selfdrive/sensord/sensors/constants.h" #include "selfdrive/sensord/sensors/sensor.h" @@ -33,4 +33,18 @@ public: virtual int init() = 0; virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0; virtual int shutdown() = 0; + + int verify_chip_id(uint8_t address, const std::vector &expected_ids) { + uint8_t chip_id = 0; + int ret = read_register(address, &chip_id, 1); + if (ret < 0) { + LOGE("Reading chip ID failed: %d", ret); + return -1; + } + for (int i = 0; i < expected_ids.size(); ++i) { + if (chip_id == expected_ids[i]) return chip_id; + } + LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]); + return -1; + } }; diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 500f41114..8a69415dd 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -19,10 +19,11 @@ CAR_VOLTAGE_LOW_PASS_K = 0.091 # LPF gain for 5s tau (dt/tau / (dt/tau + 1)) CAR_BATTERY_CAPACITY_uWh = 30e6 CAR_CHARGING_RATE_W = 45 -VBATT_PAUSE_CHARGING = 11.0 # Lower limit on the LPF car battery voltage +VBATT_PAUSE_CHARGING = 11.8 # Lower limit on the LPF car battery voltage VBATT_INSTANT_PAUSE_CHARGING = 7.0 # Lower limit on the instant car battery voltage measurements to avoid triggering on instant power loss MAX_TIME_OFFROAD_S = 30*3600 MIN_ON_TIME_S = 3600 +VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 60 class PowerMonitoring: def __init__(self): @@ -140,8 +141,8 @@ class PowerMonitoring: if self.last_measurement_time: integration_time_h = (t - self.last_measurement_time) / 3600 power_used = (current_power * 1000000) * integration_time_h - if power_used < 0: - raise ValueError(f"Negative power used! Integration time: {integration_time_h} h Current Power: {power_used} uWh") + # if power_used < 0: + # raise ValueError(f"Negative power used! Integration time: {integration_time_h} h Current Power: {power_used} uWh") self.power_used_uWh += power_used self.car_battery_capacity_uWh -= power_used self.last_measurement_time = t diff --git a/selfdrive/ui/_ui b/selfdrive/ui/_ui index 41fec3b9f..2adbb432d 100755 Binary files a/selfdrive/ui/_ui and b/selfdrive/ui/_ui differ diff --git a/selfdrive/ui/_ui_nonav b/selfdrive/ui/_ui_nonav index dd46e272e..064565c92 100755 Binary files a/selfdrive/ui/_ui_nonav and b/selfdrive/ui/_ui_nonav differ diff --git a/selfdrive/ui/qt/libpython_helpers.so b/selfdrive/ui/qt/libpython_helpers.so index 36abaded1..b26754744 100755 Binary files a/selfdrive/ui/qt/libpython_helpers.so and b/selfdrive/ui/qt/libpython_helpers.so differ diff --git a/selfdrive/ui/qt/spinner b/selfdrive/ui/qt/spinner index 8745add6c..9ca1f5658 100755 Binary files a/selfdrive/ui/qt/spinner and b/selfdrive/ui/qt/spinner differ diff --git a/selfdrive/ui/qt/text b/selfdrive/ui/qt/text index 369963967..37c3a5335 100755 Binary files a/selfdrive/ui/qt/text and b/selfdrive/ui/qt/text differ diff --git a/selfdrive/ui/soundd/_soundd b/selfdrive/ui/soundd/_soundd index 645d6f996..56af8835f 100755 Binary files a/selfdrive/ui/soundd/_soundd and b/selfdrive/ui/soundd/_soundd differ diff --git a/selfdrive/ui/translations/languages.json b/selfdrive/ui/translations/languages.json index 072d320c1..86d3e62d8 100644 --- a/selfdrive/ui/translations/languages.json +++ b/selfdrive/ui/translations/languages.json @@ -1,5 +1,6 @@ { "English": "main_en", + "Deutsch": "main_de", "Português": "main_pt-BR", "中文（繁體）": "main_zh-CHT", "中文（简体）": "main_zh-CHS", diff --git a/selfdrive/ui/translations/main_de.qm b/selfdrive/ui/translations/main_de.qm new file mode 100644 index 000000000..54eb23d64 Binary files /dev/null and b/selfdrive/ui/translations/main_de.qm differ diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts new file mode 100644 index 000000000..49b6efa9b --- /dev/null +++ b/selfdrive/ui/translations/main_de.ts @@ -0,0 +1,1943 @@ + + + + + AbstractAlert + + Close + Schließen + + + Snooze Update + Update pausieren + + + Reboot and Update + Aktualisieren und neu starten + + + + AdvancedNetworking + + Back + Zurück + + + Enable Tethering + Tethering aktivieren + + + Tethering Password + Tethering Passwort + + + EDIT + ÄNDERN + + + Enter new tethering password + Neues tethering Passwort eingeben + + + IP Address + IP Adresse + + + Enable Roaming + Roaming aktivieren + + + APN Setting + APN Einstellungen + + + Enter APN + APN eingeben + + + leave blank for automatic configuration + für automatische Konfiguration leer lassen + + + Cellular Metered + Getaktete Verbindung + + + Prevent large data uploads when on a metered connection + Hochladen großer Dateien über getaktete Verbindungen unterbinden + + + + AnnotatedCameraWidget + + km/h + km/h + + + mph + mph + + + MAX + MAX + + + SPEED + Geschwindigkeit + + + LIMIT + LIMIT + + + REAL STEER + + + + RAM PER + + + + OFF + + + + ENG RPM + + + + RELAX + + + + FAR + + + + CLOSE + + + + FOLLOW + + + + NOR + + + + SPT + + + + ECO + + + + ACCEL + + + + + C2NetworkPanel + + Wi-Fi Settings + + + + OPEN + + + + Tethering Settings + + + + IP Address + IP Adresse + + + + ConfirmationDialog + + Ok + Ok + + + Cancel + Abbrechen + + + + DPCarPanel + + Override Speed When Below + + + + Override feature will be enabled when set cruise speed is lower than this value. +1 km/h = 0.62 mph. + + + + km/h + + + + Turn On Cruise Speed Override + + + + This feature will let you set your cruise speed below vehicle standard. (usually at 26~40 km/h) + + + + Toyota / Lexus + + + + Enable SnG Mod + + + + Enable this to fix stop and go (SnG) issue on some models. +Reboot required. + + + + Enable FM Physical Button Ctrl + + + + Enable this to link Following Distance Mode (FM) control to the physical button (TSS2). +ONLY WORK ON SOME OF TSS1 VEHICLES WITH SDSU. +Reboot required. + + + + Enable AM Physical Button Ctrl + + + + Enable this to link Accel Mode (AM) control to the physical button (TSS2). +ONLY WORK ON SOME OF TSS1 VEHICLES. +Reboot required. + + + + Enable Door Auto Lock + + + + Enable this to lock doors when drive above 25 km/h. +ONLY WORK ON SOME VEHICLES. + + + + Enable Door Auto Unlock + + + + Enable this to unlock doors when shift to gear P. +ONLY WORK ON SOME VEHICLES. + + + + Enable TSS2 RAV4 Special PID Tune + + + + Enable this to use a special PID tune on 2019+ TSS2 RAV4. +Reboot Required. + + + + Enable TSS-P Prius Special Torque Tune + + + + Enable this to use a special Torque tune on PRIUS 2017 w/ bad angle sensor. +Reboot Required. + + + + Honda + + + + Enable EPS Mod Mode + + + + Enable this will increase steering, USE IT ONLY if you have a modded EPS firmware. +Reboot required. + + + + Volkswagen + + + + Enable VAG Resume Fix + + + + Enable this if your car does not auto resume (stop and go). +Reboot required. + + + + Mazda + + + + Display Below Steer Speed Alert + + + + Enable this will show below steer speed alert. +Thanks to @TheCrowd + + + + Bypass Dashcam mode + + + + Enable this to bypass dashcam mode. + + + + + DPControlPanel + + ALCA Delay + + + + Once the vehicle meets all ALCA criteria, it will wait for the seconds set here before performing lane change automatically. + + + + secs + + + + ALCA Min Speed + + + + ALCA minimum engage speed in mph. +1 mph = 1.61 km/h. + + + + mph + + + + LCA Min Speed + + + + LCA minimum engage speed in mph. +1 mph = 1.61 km/h. + + + + Lane Change Mode + + + + 1 = Lane Change Assist (LCA) +2 = Auto Lane Change Assist (ALCA) + + + + Camera Offset + + + + Adjust your camera position if your device is not mounted as per guidance. + + + + Driving Path Offset + + + + Adjust this if you wish to let openpilot drive slightly towards to left (+) or right (-) + + + + cm + + + + Use Lanelines + + + + Use Lanelines instead of End-to-End when possible. + + + + Longitudinal + + + + Manually Control Accel Mode + + + + Enable this if you wish to adjust openpilot's acceleration control. + + + + Enable vision based turn control + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + Manually Control Following Dist. Mode + + + + Enable this if you wish to adjust openpilot's following distance. +openpilot by default keeps 1.45 secs distance to lead car. +When on close will be dynamic but get closer in traffic. Normal is also dynamic and it get further and far is stock 1.45sec + + + + DE2E Adapt Following Mode + + + + Enable this if you wish to use following dist. mode in DE2E. + + + + DE2E Adapt Accel Mode + + + + Enable this if you wish to use accel mode in DE2E. + + + + DE2E w/ VOACC + + + + Enable this if your vehicles is in VOACC (e.g. Honda Bosch / VAG). + + + + Dynamic End-to-end + + + + Automatically Turn On and Off End-to-end longitudinal (extremely alpha), this will ignore the stock end-to-end settings + + + + Lateral + + + + Planner Version + + + + Override the default planner version. +1 = 0.8.13 +2 = 0.8.16 + + + + LATEST + + + + Controller Type + + + + Override the default controller. +1 = PID +2 = LQR +3 = Torque +Your Vehicle may not support all the options, YMMV. + + + + DEFAULT + + + + Enable Torque Ctrl Auto Tune + + + + Enable auto tune Torque controller. +WORKS WELL ONLY ON SOME VEHICLES. +More linear steering experience. + + + + Always On Lateral + + + + Use at your own risk! Your drive will not upload but you can find them under /data/media/0/fakedata you will not be ban but we just don't upload since comma does not use data from fork but it will be stored locally. +1 = Stock Long +2 = OP Long +Reboot required. + + + + OFF + + + + Enable Device Temp Check + + + + Enable Max Ctrl Speed Check + + + + + DPGeneralPanel + + Auto Shutdown In + + + + Adjust your shutdown waiting period. +0 = shutdown immediately. + + + + mins + + + + Enable Auto Shutdown + + + + Enable this if you wish to shutdown your device automatically. + + + + Use Custom API Server + + + + Enable this if you wish to connect to a custom API server. +Default to "https://api.retropilot.org/", change "dp_api_custom" if you want to change API server URL. +Reboot required. + + + + Services + + + + Enable On-Road Dashcam + + + + Enable this if you wish to record screen, just like a dashcam. +Reboot required. + + + + Enable GPS Logger + + + + This will store your track in /data/media/0/gpx_logs/. +Reboot required. + + + + Hardware - General + + + + Enable Hotspot On Boot + + + + Reboot required. + + + + Miscellaneous + + + + Enable Driver Monitor (Front Cam) + + + + Enable this option if you wish to use front camera for driver monitoring. +On = Front Cam Monitor + Steering Monitor +Off = Steering Monitor only. +Enable this will increase device temp a lot, use at your own risk. +Reboot Required. + + + + Reboot recommended. +Reboot? + + + + Reboot + Neustart + + + Enable Jetson Support + + + + Enable this option if you intend to run dp on Nvidia Jetson. +Reboot required. + + + + Flashing Panda Firmware + + + + FLASH + + + + Tap the button to update your panda firmware. +The device should reboot once if it finish updating. + + + + Are you sure you want to flash the panda? + + + + Pandas Firmware Recovery + + + + RECOVER + + + + Tap the button ONLY if your panda ran into issue. + + + + Are you sure you want to run recover panda script? + + + + Reset dragonpilot conf + + + + RESET + RESET + + + Tap the button to reset all your dragonpilot congiration to default value. +Reboot required. + + + + Are you sure you want to reset dp params? + + + + Delete All Driving Log + + + + DELETE + + + + Tap the button to delete ALL your driving logs (including dashcam / gpx / driving logs). + + + + Are you sure delete all media? + + + + + DPMapsPanel + + Show Full Screen Nav. + + + + This will show navigation in full screen. +Please tap green boarder if you wish to switch back drive view. + + + + Search Destination using Google Map + + + + This will allow you to search destination in google map api. +You will need to apply your own google map api key.Enter your key detail in web interface once it's enabled. + + + + Enable Local Nav Server + + + + This will let use Navigation feature with your own access key. +Use web interface to control it: *http://<device_ip>:8082*. +You will need to apply your own mapbox token at https://www.mapbox.com/. +Internet access from mobile phone (tethering) is required. +Reboot required. + + + + Reboot recommended. +Reboot? + + + + Enable Nav. + + + + This will let use the build in Navigation. +Reboot required. + + + + Enable Speed Limit Control + + + + Use speed limit signs information from map data and car interface to automatically adapt cruise speed to road limits. + + + + Enable Speed Limit Offset + + + + Set speed limit slightly higher than actual speed limit for a more natural drive. + + + + Enable Map Data Turn Control + + + + Use curvature info from map data to define speed limits to take turns ahead + + + + Show debug UI elements + + + + Show UI elements that aid debugging. + + + + Enable MapD + + + + Use OSM to assist lateral/longitudinal control. +Please note: +1. This feature will works only when your car support OP longitudinal. +2. MapD will contribute your route to OSM for future improvement automatically. +3. You can add your own offset for mapd just follow the readme under /selfdrive/mapd. +4. Not connecting to the internet for while might feel up device storage from all the gps traces. + + + + + DPUIPanel + + Display Mode + + + + 0 = Default +1 = Screen Off While Driving + + + + Screen Brightness + + + + Adjust your screen brightness. + + + + AUTO + + + + Alert Volume + + + + Adjust your alert volume. + + + + Quiet Drive + + + + Display alert and play important warning sound. Thanks @sunnyhaibin. + + + + Display Speed + + + + Enable this to display your current speed. + + + + Display Event / Steer Icon + + + + Enable this to display the icon. + + + + Display Driver Monitor Indicator + + + + Display Side Info + + + + Enable this to display steering angle / lead car distance / engine RPM. + + + + Display Top Info + + + + Enable this to display time / system temp / battery level. + + + + Display Lead Speed/Distance + + + + Display detected lead objects' speed and distance. + + + + Volume + + + + LAUNCH + + + + Date/Time + + + + + DeclinePage + + You must accept the Terms and Conditions in order to use openpilot. + Du musst die Nutzungsbedingungen akzeptieren, um Openpilot zu benutzen. + + + Back + Zurück + + + Decline, uninstall %1 + Ablehnen, deinstallieren %1 + + + + DevicePanel + + Dongle ID + Dongle ID + + + N/A + Nicht verfügbar + + + Serial + Seriennummer + + + Driver Camera + Fahrerkamera + + + PREVIEW + VORSCHAU + + + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) + Vorschau der auf den Fahrer gerichteten Kamera, um sicherzustellen, dass die Fahrerüberwachung eine gute Sicht hat. (Fahrzeug muss aus sein) + + + Reset Calibration + Neu kalibrieren + + + RESET + RESET + + + Are you sure you want to reset calibration? + Bist du sicher, dass du die Kalibrierung zurücksetzen möchtest? + + + Review Training Guide + Trainingsanleitung wiederholen + + + REVIEW + TRAINING + + + Review the rules, features, and limitations of openpilot + Wiederhole die Regeln, Fähigkeiten und Limitierungen von Openpilot + + + Are you sure you want to review the training guide? + Bist du sicher, dass du die Trainingsanleitung wiederholen möchtest? + + + Regulatory + Rechtliche Hinweise + + + VIEW + ANSEHEN + + + Change Language + Sprache ändern + + + CHANGE + ÄNDERN + + + Select a language + Sprache wählen + + + Reboot + Neustart + + + Power Off + Ausschalten + + + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. + Damit Openpilot funktioniert, darf die Installationsposition nicht mehr als 4° nach rechts/links, 5° nach oben und 8° nach unten abweichen. Openpilot kalibriert sich durchgehend, ein Zurücksetzen ist selten notwendig. + + + Your device is pointed %1° %2 and %3° %4. + Deine Geräteausrichtung ist %1° %2 und %3° %4. + + + down + unten + + + up + oben + + + left + links + + + right + rechts + + + Are you sure you want to reboot? + Bist du sicher, dass du das Gerät neu starten möchtest? + + + Disengage to Reboot + Für Neustart deaktivieren + + + Are you sure you want to power off? + Bist du sicher, dass du das Gerät ausschalten möchtest? + + + Disengage to Power Off + Zum Ausschalten deaktivieren + + + Reset + Zurücksetzen + + + Review + Überprüfen + + + + DriveStats + + Drives + Fahrten + + + Hours + Stunden + + + ALL TIME + Gesamtzeit + + + PAST WEEK + Letzte Woche + + + KM + KM + + + Miles + Meilen + + + + DriverViewScene + + camera starting + Kamera startet + + + + ExperimentalModeButton + + EXPERIMENTAL MODE ON + EXPERIMENTELLER MODUS AN + + + CHILL MODE ON + ENTSPANNTER MODUS AN + + + EXPERIMENTAL MODE / DE2E ON + + + + + InputDialog + + Cancel + Abbrechen + + + Need at least %n character(s)! + + Mindestens %n Buchstabe benötigt! + Mindestens %n Buchstaben benötigt! + + + + + Installer + + Installing... + Installiere... + + + Receiving objects: + Empfange Objekte: + + + Resolving deltas: + Unterschiede verarbeiten: + + + Updating files: + Dateien aktualisieren: + + + + MapETA + + eta + Ankunft + + + min + min + + + hr + std + + + km + km + + + mi + mi + + + + MapInstructions + + km + km + + + m + m + + + mi + mi + + + ft + fuß + + + + MapPanel + + Current Destination + Aktuelles Ziel + + + CLEAR + LÖSCHEN + + + Recent Destinations + Letzte Ziele + + + Try the Navigation Beta + Beta Navigation ausprobieren + + + Get turn-by-turn directions displayed and more with a comma +prime subscription. Sign up now: https://connect.comma.ai + Erhalte echtzeit Wegführung und mehr mit dem comma prime +Abonnement. Melde dich jetzt an: https://connect.comma.ai + + + No home +location set + Keine Heimadresse gesetzt + + + No work +location set + Keine Arbeitsadresse gesetzt + + + no recent destinations + Keine kürzlich gewählten Ziele + + + + MapWindow + + Map Loading + Karte wird geladen + + + Waiting for GPS + Warten auf GPS + + + + MultiOptionDialog + + Select + Auswählen + + + Cancel + Abbrechen + + + + Networking + + Advanced + Erweitert + + + Enter password + Passwort eingeben + + + for "%1" + für "%1" + + + Wrong password + Falsches Passwort + + + + OffroadHome + + UPDATE + Aktualisieren + + + ALERTS + HINWEISE + + + ALERT + HINWEIS + + + + PairingPopup + + Pair your device to your comma account + Verbinde dein Gerät mit deinem comma Konto + + + Go to https://connect.comma.ai on your phone + Gehe zu https://connect.comma.ai auf deinem Handy + + + Click "add new device" and scan the QR code on the right + Klicke auf "neues Gerät hinzufügen" und scanne den QR code rechts + + + Bookmark connect.comma.ai to your home screen to use it like an app + Füge connect.comma.ai als Lesezeichen auf deinem Homescreen hinzu um es wie eine App zu verwenden + + + + ParamControl + + Cancel + Abbrechen + + + Enable + Aktivieren + + + + PrimeAdWidget + + Upgrade Now + Jetzt abonieren + + + Become a comma prime member at connect.comma.ai + Werde Comma Prime Mitglied auf connect.comma.ai + + + PRIME FEATURES: + PRIME FUNKTIONEN: + + + Remote access + Fernzugriff + + + 1 year of storage + 1 Jahr Speicherplatz + + + Developer perks + Entwickler Vorteile + + + + PrimeUserWidget + + ✓ SUBSCRIBED + ✓ ABBONIERT + + + comma prime + comma prime + + + CONNECT.COMMA.AI + CONNECT.COMMA.AI + + + COMMA POINTS + COMMA PUNKTE + + + + QObject + + Reboot + Neustart + + + Exit + Verlassen + + + dashcam + dashcam + + + openpilot + openpilot + + + %n minute(s) ago + + vor %n Minute + vor %n Minuten + + + + %n hour(s) ago + + vor %n Stunde + vor %n Stunden + + + + %n day(s) ago + + vor %n Tag + vor %n Tagen + + + + + Reset + + Reset failed. Reboot to try again. + Zurücksetzen fehlgeschlagen. Starte das Gerät neu und versuche es wieder. + + + Are you sure you want to reset your device? + Bist du sicher, dass du das Gerät auf Werkseinstellungen zurücksetzen möchtest? + + + Resetting device... + Gerät wird zurückgesetzt... + + + System Reset + System auf Werkseinstellungen zurücksetzen + + + System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. + Zurücksetzen auf Werkseinstellungen wurde ausgewählt. Drücke Annehmen, um alle Inhalte und Einstellungen zu löschen. Drücke Abbrechen, um mit dem Starten des Gerätes fortzufahren. + + + Cancel + Abbrechen + + + Reboot + Neustart + + + Confirm + Bestätigen + + + Unable to mount data partition. Press confirm to reset your device. + Datenpartition kann nicht geöffnet werden. Drücke Annehmen, um dein Gerät auf Werkseinstellungen zurückzusetzen. + + + + SettingsWindow + + × + x + + + Device + Gerät + + + Network + Netzwerk + + + Toggles + Schalter + + + Software + Software + + + Navigation + Navigation + + + BACK + + + + DP - General + + + + DP - Controls + + + + DP - UI + + + + DP - Cars + + + + DP - Maps + + + + + Setup + + WARNING: Low Voltage + Warnung: Batteriespannung niedrig + + + Power your device in a car with a harness or proceed at your own risk. + Versorge dein Gerät über einen Kabelbaum im Auto mit Strom, oder fahre auf eigene Gefahr fort. + + + Power off + Ausschalten + + + Continue + Fortsetzen + + + Getting Started + Loslegen + + + Before we get on the road, let’s finish installation and cover some details. + Bevor wir uns auf die Straße begeben, lass uns die Installation fertigstellen und einige Details prüfen. + + + Connect to Wi-Fi + Mit WLAN verbinden + + + Back + Zurück + + + Continue without Wi-Fi + Ohne WLAN fortsetzen + + + Waiting for internet + Auf Internet warten + + + Choose Software to Install + Software zum installieren auswählen + + + Dashcam + Dashcam + + + Custom Software + Spezifische Software + + + Enter URL + URL eingeben + + + for Custom Software + für spezifische Software + + + Downloading... + Herunterladen... + + + Download Failed + Herunterladen fehlgeschlagen + + + Ensure the entered URL is valid, and the device’s internet connection is good. + Stelle sicher, dass die eingegebene URL korrekt ist und dein Gerät eine stabile Internetverbindung hat. + + + Reboot device + Gerät neustarten + + + Start over + Von neuem beginnen + + + + SetupWidget + + Finish Setup + Einrichtung beenden + + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Koppele dein Gerät mit Comma Connect (connect.comma.ai) und sichere dir dein Comma Prime Angebot. + + + Pair device + Gerät koppeln + + + + Sidebar + + CONNECT + This is a brand/service name for comma connect, don't translate + CONNECT + + + OFFLINE + OFFLINE + + + ONLINE + ONLINE + + + ERROR + FEHLER + + + TEMP + TEMP + + + HIGH + HOCH + + + GOOD + GUT + + + OK + OK + + + VEHICLE + FAHRZEUG + + + NO + KEIN + + + PANDA + PANDA + + + GPS + GPS + + + SEARCH + SUCHEN + + + -- + -- + + + Wi-Fi + WLAN + + + ETH + LAN + + + 2G + 2G + + + 3G + 3G + + + LTE + LTE + + + 5G + 5G + + + %1 m + + + + + SoftwarePanel + + UNINSTALL + Too long for UI + DEINSTALL + + + Uninstall %1 + Deinstalliere %1 + + + Are you sure you want to uninstall? + Bist du sicher, dass du Openpilot entfernen möchtest? + + + CHECK + ÜBERPRÜFEN + + + Updates are only downloaded while the car is off. + Updates werden nur heruntergeladen, wenn das Auto aus ist. + + + Current Version + Aktuelle Version + + + Download + Download + + + Install Update + Update installieren + + + INSTALL + INSTALLIEREN + + + Target Branch + Ziel Branch + + + SELECT + AUSWÄHLEN + + + Select a branch + Wähle einen Branch + + + Uninstall + Deinstallieren + + + OpenStreetMap Database Update + + + + "OpenStreetMap Database Update" +Your device will reboot and perform a database update based on the region you have selected. +Reboot now? + + + + OpenStreetMap Database + + + + Select your location + + + + "OpenStreetMap Database" +Restart your device to start downloading the selected database. Dragonpilot will only start downloading the database on reboot. +Reboot now? + + + + + SshControl + + SSH Keys + SSH Schlüssel + + + Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own. A comma employee will NEVER ask you to add their GitHub username. + Warnung: Dies ermöglicht SSH zugriff für alle öffentlichen Schlüssel in deinen Github Einstellungen. Gib niemals einen anderen Benutzernamen, als deinen Eigenen an. Comma Angestellte fragen dich niemals danach ihren Github Benutzernamen hinzuzufügen. + + + ADD + HINZUFÜGEN + + + Enter your GitHub username + Gib deinen GitHub Benutzernamen ein + + + LOADING + LADEN + + + REMOVE + LÖSCHEN + + + Username '%1' has no keys on GitHub + Benutzername '%1' hat keine Schlüssel auf GitHub + + + Request timed out + Zeitüberschreitung der Anforderung + + + Username '%1' doesn't exist on GitHub + Benutzername '%1' existiert nicht auf GitHub + + + + SshToggle + + Enable SSH + SSH aktivieren + + + + TermsPage + + Terms & Conditions + Benutzungsbedingungen + + + Decline + Ablehnen + + + Scroll to accept + Scrolle herunter um zu akzeptieren + + + Agree + Zustimmen + + + + TogglesPanel + + Enable openpilot + Openpilot aktivieren + + + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. + Benutze das Openpilot System als adaptiven Tempomaten und Spurhalteassistenten. Deine Aufmerksamkeit ist jederzeit erforderlich, um diese Funktion zu nutzen. Diese Einstellung wird übernommen, wenn das Auto aus ist. + + + Enable Lane Departure Warnings + Spurverlassenswarnungen aktivieren + + + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). + Erhalte Warnungen, zurück in die Spur zu lenken, wenn dein Auto über eine erkannte Fahrstreifenmarkierung ohne aktivierten Blinker mit mehr als 50 km/h fährt. + + + Use Metric System + Benutze das metrische System + + + Display speed in km/h instead of mph. + Zeige die Geschwindigkeit in km/h anstatt von mph. + + + Record and Upload Driver Camera + Fahrerkamera aufnehmen und hochladen + + + Upload data from the driver facing camera and help improve the driver monitoring algorithm. + Lade Daten der Fahreraufmerksamkeitsüberwachungskamera hoch, um die Fahreraufmerksamkeitsüberwachungsalgorithmen zu verbessern. + + + When enabled, pressing the accelerator pedal will disengage openpilot. + Wenn aktiviert, deaktiviert sich Openpilot sobald das Gaspedal betätigt wird. + + + Use 24h format instead of am/pm + Benutze das 24Stunden Format anstatt am/pm + + + Show Map on Left Side of UI + Too long for UI + Zeige die Karte auf der linken Seite + + + Show map on left side when in split screen view. + Zeige die Karte auf der linken Seite der Benutzeroberfläche bei geteilten Bildschirm. + + + Show ETA in 24h Format + Too long for UI + Zeige die Ankunftszeit im 24 Stunden Format + + + Experimental Mode + Experimenteller Modus + + + Experimental openpilot Longitudinal Control + Experimenteller Openpilot Tempomat + + + WARNING: openpilot longitudinal control is experimental for this car and will disable Automatic Emergency Braking (AEB). + WARNUNG: Der Openpilot Tempomat ist für dieses Auto experimentell und deaktiviert den Notbremsassistenten. + + + Disengage on Accelerator Pedal + Bei Gasbetätigung ausschalten + + + On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when using experimental openpilot longitudinal control. + Bei diesem auto wird standardmäßig der im Auto eingebaute adaptive Tempomat anstelle des Openpilot Tempomats benutzt. Aktiviere diesen Schalter, um zum Openpilot Tempomaten zu wechseln. Es ist empfohlen den Experimentellen Modus bei Nutzung des Openpilot Tempomats zu aktivieren. + + + openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: + Openpilot fährt standardmäßig im <b>entspannten Modus</b>. Der Experimentelle Modus aktiviert<b>Alpha-level Funktionen</b>, die noch nicht für den entspannten Modus bereit sind. Die experimentellen Funktionen sind die Folgenden: + + + 🌮 End-to-End Longitudinal Control 🌮 + 🌮 Ende-zu-Ende Tempomat 🌮 + + + Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. + Lass das Fahrmodell Gas und Bremse kontrollieren. Openpilot wird so fahren, wie es dies von einem Menschen erwarten würde; inklusive des Anhaltens für Ampeln und Stoppschildern. Da das Fahrmodell entscheidet wie schnell es fährt stellt die gesetzte Geschwindigkeit lediglich das obere Limit dar. Dies ist ein Alpha-level Funktion. Fehler sind zu erwarten. + + + New Driving Visualization + Neue Fahrvisualisierung + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + Die Fahrvisualisierung wechselt bei niedrigen Geschwindigkeiten zur Straßengewandten Weitwinkelkamera, um manche Kurven besser zu zeigen. Außerdem wird das Experimenteller Modus logo oben rechts angezeigt. + + + Experimental mode is currently unavailable on this car, since the car's stock ACC is used for longitudinal control. + Der experimentelle Modus ist momentan für dieses Auto nicht verfügbar, da es den eingebauten adaptiven Tempomaten des Autos benutzt. + + + Enable experimental longitudinal control to allow experimental mode. + Aktiviere den experimentellen Openpilot Tempomaten für experimentelle Funktionen. + + + Enable Right-Hand Drive + + + + Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat. + + + + + Updater + + Update Required + Aktualisierung notwendig + + + An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. + Eine Aktualisierung des Betriebssystems ist notwendig. Verbinde dein Gerät mit WLAN für ein schnelleres Update. Die Download Größe ist ungefähr 1GB. + + + Connect to Wi-Fi + Mit WLAN verbinden + + + Install + Installieren + + + Back + Zurück + + + Loading... + Laden... + + + Reboot + Neustart + + + Update failed + Aktualisierung fehlgeschlagen + + + + WifiUI + + Scanning for networks... + Suche nach Netzwerken... + + + CONNECTING... + VERBINDEN... + + + FORGET + VERGESSEN + + + Forget Wi-Fi Network "%1"? + WLAN Netzwerk "%1" vergessen? + + + Forget + Vergessen + + + diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 821853dd4..06bae7b2e 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -486,28 +486,6 @@ More linear steering experience. Reboot required. - - when acc set speed below the setting and there is no lead car, e2e will be turned on automatically. -1 km/h = 0.62 mph - - - - km/h - - - - when lead car is going below the setting, e2e will be turned on automatically. -1 km/h = 0.62 mph - - - - DE2E When Car Set Speed Below (w/o Lead): - - - - DE2E When Lead Car Speed Below: - - DE2E Adapt Following Mode @@ -524,6 +502,14 @@ Reboot required. Enable this if you wish to use accel mode in DE2E. + + DE2E w/ VOACC + + + + Enable this if your vehicles is in VOACC (e.g. Honda Bosch / VAG). + + DPGeneralPanel @@ -784,17 +770,6 @@ Please note: 4. Not connecting to the internet for while might feel up device storage from all the gps traces. - - Use Mapd without data. - - - - You need minimum of 50 gb storage in /data/media/0/. - Run df -h /data/media/0/ to see how much space you have available. - Strongly recommend getting 1 TB ssd. - If you decide not to upgrade you can delete all logs under dp-general - - DPUIPanel @@ -1641,6 +1616,10 @@ location set 5G 5G + + %1 m + + SoftwarePanel diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index 8e5c438c5..2216056fa 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -486,28 +486,6 @@ More linear steering experience. Reboot required. - - when acc set speed below the setting and there is no lead car, e2e will be turned on automatically. -1 km/h = 0.62 mph - - - - km/h - - - - when lead car is going below the setting, e2e will be turned on automatically. -1 km/h = 0.62 mph - - - - DE2E When Car Set Speed Below (w/o Lead): - - - - DE2E When Lead Car Speed Below: - - DE2E Adapt Following Mode @@ -524,6 +502,14 @@ Reboot required. Enable this if you wish to use accel mode in DE2E. + + DE2E w/ VOACC + + + + Enable this if your vehicles is in VOACC (e.g. Honda Bosch / VAG). + + DPGeneralPanel @@ -784,17 +770,6 @@ Please note: 4. Not connecting to the internet for while might feel up device storage from all the gps traces. - - Use Mapd without data. - - - - You need minimum of 50 gb storage in /data/media/0/. - Run df -h /data/media/0/ to see how much space you have available. - Strongly recommend getting 1 TB ssd. - If you decide not to upgrade you can delete all logs under dp-general - - DPUIPanel @@ -1641,6 +1616,10 @@ location set 5G 5G + + %1 m + + SoftwarePanel diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 781dee577..51aad502b 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -486,28 +486,6 @@ More linear steering experience. Reboot required. - - when acc set speed below the setting and there is no lead car, e2e will be turned on automatically. -1 km/h = 0.62 mph - - - - km/h - - - - when lead car is going below the setting, e2e will be turned on automatically. -1 km/h = 0.62 mph - - - - DE2E When Car Set Speed Below (w/o Lead): - - - - DE2E When Lead Car Speed Below: - - DE2E Adapt Following Mode @@ -524,6 +502,14 @@ Reboot required. Enable this if you wish to use accel mode in DE2E. + + DE2E w/ VOACC + + + + Enable this if your vehicles is in VOACC (e.g. Honda Bosch / VAG). + + DPGeneralPanel @@ -784,17 +770,6 @@ Please note: 4. Not connecting to the internet for while might feel up device storage from all the gps traces. - - Use Mapd without data. - - - - You need minimum of 50 gb storage in /data/media/0/. - Run df -h /data/media/0/ to see how much space you have available. - Strongly recommend getting 1 TB ssd. - If you decide not to upgrade you can delete all logs under dp-general - - DPUIPanel @@ -1645,6 +1620,10 @@ trabalho definido 5G 5G + + %1 m + + SoftwarePanel diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index cdb27e660..ea5fee986 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -486,28 +486,6 @@ More linear steering experience. Reboot required. - - when acc set speed below the setting and there is no lead car, e2e will be turned on automatically. -1 km/h = 0.62 mph - - - - km/h - - - - when lead car is going below the setting, e2e will be turned on automatically. -1 km/h = 0.62 mph - - - - DE2E When Car Set Speed Below (w/o Lead): - - - - DE2E When Lead Car Speed Below: - - DE2E Adapt Following Mode @@ -524,6 +502,14 @@ Reboot required. Enable this if you wish to use accel mode in DE2E. + + DE2E w/ VOACC + + + + Enable this if your vehicles is in VOACC (e.g. Honda Bosch / VAG). + + DPGeneralPanel @@ -784,17 +770,6 @@ Please note: 4. Not connecting to the internet for while might feel up device storage from all the gps traces. - - Use Mapd without data. - - - - You need minimum of 50 gb storage in /data/media/0/. - Run df -h /data/media/0/ to see how much space you have available. - Strongly recommend getting 1 TB ssd. - If you decide not to upgrade you can delete all logs under dp-general - - DPUIPanel @@ -1639,6 +1614,10 @@ location set 5G 5G + + %1 m + + SoftwarePanel diff --git a/selfdrive/ui/translations/main_zh-CHT.qm b/selfdrive/ui/translations/main_zh-CHT.qm index a59b92bfa..a6c138a10 100644 Binary files a/selfdrive/ui/translations/main_zh-CHT.qm and b/selfdrive/ui/translations/main_zh-CHT.qm differ diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 9e137eef2..f1063d4e4 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -440,7 +440,7 @@ When on close will be dynamic but get closer in traffic. Normal is also dynamic DE2E When Lead Car Speed Below: - 切換至 E2E 當前車車速低於： + 切換至 E2E 當前車車速低於： Dynamic End-to-end @@ -448,7 +448,7 @@ When on close will be dynamic but get closer in traffic. Normal is also dynamic Automatically Turn On and Off End-to-end longitudinal (extremely alpha), this will ignore the stock end-to-end settings - 自動開啟、關閉 E2E 縱向控制 (實驗功能)，這將會忽略 OP 本來的 E2E 設定。 + 自動開啟、關閉 E2E 縱向控制 (實驗功能) Lateral @@ -528,7 +528,7 @@ Reboot required. km/h - km/h + km/h when lead car is going below the setting, e2e will be turned on automatically. @@ -538,7 +538,7 @@ Reboot required. DE2E When Car Set Speed Below (w/o Lead): - 切換至 E2E 當定速低於 (無前車)： + 切換至 E2E 當定速低於 (無前車)： E2E When Car Set Speed Below (w/o Lead): @@ -567,15 +567,23 @@ Reboot required. when acc set speed below the setting and there is no lead car, e2e will be turned on automatically. 1 km/h = 0.62 mph - 當車子的定速低於設定值並且在沒有前車的情形下，系統會自動開啟 E2E 模式。 + 當車子的定速低於設定值並且在沒有前車的情形下，系統會自動開啟 E2E 模式。 1 km/h = 0.62 mph when lead car is going below the setting, e2e will be turned on automatically. 1 km/h = 0.62 mph - 當前車車速於低於設定值時，系統將自動切換至 E2E 模式。 + 當前車車速於低於設定值時，系統將自動切換至 E2E 模式。 1 km/h = 0.62 mph + + DE2E w/ VOACC + DE2E 用於 VOACC 下 + + + Enable this if your vehicles is in VOACC (e.g. Honda Bosch / VAG). + 如果您的車子使用的是 VOACC (例如：Honda Bosch / VAG/HKG)，可以開啟這個功能增進剎車效能。 + DPGeneralPanel @@ -860,14 +868,14 @@ Please note: Use Mapd without data. - 使用本地端 Mapd 圖資 + 使用本地端 Mapd 圖資 You need minimum of 50 gb storage in /data/media/0/. Run df -h /data/media/0/ to see how much space you have available. Strongly recommend getting 1 TB ssd. If you decide not to upgrade you can delete all logs under dp-general - 您設備的 /data/media/0/ 將需要至少 50GB 的存儲空間。 + 您設備的 /data/media/0/ 將需要至少 50GB 的存儲空間。 @@ -1724,6 +1732,10 @@ location set 5G + + %1 m + %1 m + SoftwarePanel diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index d4ea70cea..3bf146688 100755 --- a/selfdrive/ui/ui +++ b/selfdrive/ui/ui @@ -2,9 +2,14 @@ cd "$(dirname "$0")" export LD_LIBRARY_PATH="/system/lib64:/data/openpilot/third_party/mapbox-gl-native-qt/aarch64:$LD_LIBRARY_PATH" export QT_DBL_CLICK_DIST=150 -NAV=`cat /data/params/d/dp_nav` -if [ -f /data/params/d/dp_nav ] && [ $NAV != "0" ]; then - exec ./_ui +if [ -f /data/params/d/dp_nav ]; then + NAV=`cat /data/params/d/dp_nav` else - exec ./_ui_nonav + NAV="0" +fi + +if [ $NAV == "0" ] && [ -f /data/openpilot/selfdrive/ui/_ui_nonav ]; then + exec ./_ui_nonav +else + exec ./_ui fi diff --git a/system/camerad/camerad b/system/camerad/camerad index a8b1df485..6e0251f60 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/clocksd/clocksd b/system/clocksd/clocksd index 47fbd8b44..6ac77fc2c 100755 Binary files a/system/clocksd/clocksd and b/system/clocksd/clocksd differ diff --git a/system/hardware/base.h b/system/hardware/base.h index b70948d48..f6e0b42d7 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -20,6 +20,7 @@ public: static void poweroff() {} static void set_brightness(int percent) {} static void set_display_power(bool on) {} + static void set_volume(float volume) {} static bool get_ssh_enabled() { return false; } static void set_ssh_enabled(bool enabled) {} diff --git a/system/hardware/hw.h b/system/hardware/hw.h index 8f61f54ee..b8b223e8e 100644 --- a/system/hardware/hw.h +++ b/system/hardware/hw.h @@ -11,15 +11,7 @@ #include "system/hardware/eon/hardware.h" #define Hardware HardwareEon #else -class HardwarePC : public HardwareNone { -public: - static std::string get_os_version() { return "openpilot for PC"; } - static std::string get_name() { return "pc"; }; - static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; }; - static bool PC() { return true; } - static bool TICI() { return util::getenv("TICI", 0) == 1; } - static bool AGNOS() { return util::getenv("TICI", 0) == 1; } -}; +#include "system/hardware/pc/hardware.h" #define Hardware HardwarePC #endif @@ -28,11 +20,8 @@ inline std::string log_root() { if (const char *env = getenv("LOG_ROOT")) { return env; } - if (std::atoi(Params().get("dp_atl").c_str()) > 0 || Params().getBool("dp_jetson") || Params().getBool("dp_api_custom")) { - return "/data/media/0/fakedata"; - } else { - return Hardware::PC() ? util::getenv("HOME") + "/.comma/media/0/realdata" : "/data/media/0/realdata"; - } + return "/data/media/0/fakedata"; + //return Hardware::PC() ? util::getenv("HOME") + "/.comma/media/0/realdata" : "/data/media/0/realdata"; } inline std::string params() { return Hardware::PC() ? util::getenv("HOME") + "/.comma/params" : "/data/params"; diff --git a/system/hardware/pc/hardware.h b/system/hardware/pc/hardware.h new file mode 100644 index 000000000..529b4bfe9 --- /dev/null +++ b/system/hardware/pc/hardware.h @@ -0,0 +1,21 @@ +#pragma once + +#include "system/hardware/base.h" + +class HardwarePC : public HardwareNone { +public: + static std::string get_os_version() { return "openpilot for PC"; } + static std::string get_name() { return "pc"; }; + static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; }; + static bool PC() { return true; } + static bool TICI() { return util::getenv("TICI", 0) == 1; } + static bool AGNOS() { return util::getenv("TICI", 0) == 1; } + + static void set_volume(float volume) { + volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME); + + char volume_str[6]; + snprintf(volume_str, sizeof(volume_str), "%.3f", volume); + std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str()); + } +}; diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index dcccb9f3d..d388f9c48 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -10,7 +10,7 @@ class HardwareTici : public HardwareNone { public: static constexpr float MAX_VOLUME = 0.9; - static constexpr float MIN_VOLUME = 0.2; + static constexpr float MIN_VOLUME = 0.1; static bool TICI() { return true; } static bool AGNOS() { return true; } static std::string get_os_version() { @@ -38,6 +38,13 @@ public: bl_power_control.close(); } }; + static void set_volume(float volume) { + volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME); + + char volume_str[6]; + snprintf(volume_str, sizeof(volume_str), "%.3f", volume); + std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str()); + } static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }; static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }; diff --git a/system/micd.py b/system/micd.py new file mode 100755 index 000000000..a56140e3b --- /dev/null +++ b/system/micd.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +import sounddevice as sd +import numpy as np + +from cereal import messaging +from common.filter_simple import FirstOrderFilter +from common.realtime import Ratekeeper +from system.swaglog import cloudlog + +RATE = 10 +FFT_SAMPLES = 4096 +REFERENCE_SPL = 2e-5 # newtons/m^2 +SAMPLE_RATE = 44100 +FILTER_DT = 1. / (SAMPLE_RATE / FFT_SAMPLES) + + +def calculate_spl(measurements): + # https://www.engineeringtoolbox.com/sound-pressure-d_711.html + sound_pressure = np.sqrt(np.mean(measurements ** 2)) # RMS of amplitudes + if sound_pressure > 0: + sound_pressure_level = 20 * np.log10(sound_pressure / REFERENCE_SPL) # dB + else: + sound_pressure_level = 0 + return sound_pressure, sound_pressure_level + + +def apply_a_weighting(measurements: np.ndarray) -> np.ndarray: + # Generate a Hanning window of the same length as the audio measurements + measurements_windowed = measurements * np.hanning(len(measurements)) + + # Calculate the frequency axis for the signal + freqs = np.fft.fftfreq(measurements_windowed.size, d=1 / SAMPLE_RATE) + + # Calculate the A-weighting filter + # https://en.wikipedia.org/wiki/A-weighting + A = 12194 ** 2 * freqs ** 4 / ((freqs ** 2 + 20.6 ** 2) * (freqs ** 2 + 12194 ** 2) * np.sqrt((freqs ** 2 + 107.7 ** 2) * (freqs ** 2 + 737.9 ** 2))) + A /= np.max(A) # Normalize the filter + + # Apply the A-weighting filter to the signal + return np.abs(np.fft.ifft(np.fft.fft(measurements_windowed) * A)) + + +class Mic: + def __init__(self, pm): + self.pm = pm + self.rk = Ratekeeper(RATE) + + self.measurements = np.empty(0) + + self.sound_pressure = 0 + self.sound_pressure_weighted = 0 + self.sound_pressure_level_weighted = 0 + + self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) + + def update(self): + msg = messaging.new_message('microphone') + msg.microphone.soundPressure = float(self.sound_pressure) + msg.microphone.soundPressureWeighted = float(self.sound_pressure_weighted) + + msg.microphone.soundPressureWeightedDb = float(self.sound_pressure_level_weighted) + msg.microphone.filteredSoundPressureWeightedDb = float(self.spl_filter_weighted.x) + + self.pm.send('microphone', msg) + self.rk.keep_time() + + def callback(self, indata, frames, time, status): + """ + Using amplitude measurements, calculate an uncalibrated sound pressure and sound pressure level. + Then apply A-weighting to the raw amplitudes and run the same calculations again. + + Logged A-weighted equivalents are rough approximations of the human-perceived loudness. + """ + + self.measurements = np.concatenate((self.measurements, indata[:, 0])) + + while self.measurements.size >= FFT_SAMPLES: + measurements = self.measurements[:FFT_SAMPLES] + + self.sound_pressure, _ = calculate_spl(measurements) + measurements_weighted = apply_a_weighting(measurements) + self.sound_pressure_weighted, self.sound_pressure_level_weighted = calculate_spl(measurements_weighted) + self.spl_filter_weighted.update(self.sound_pressure_level_weighted) + + self.measurements = self.measurements[FFT_SAMPLES:] + + def micd_thread(self, device=None): + if device is None: + device = "sysdefault" + + with sd.InputStream(device=device, channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: + cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}") + while True: + self.update() + + +def main(pm=None): + if pm is None: + pm = messaging.PubMaster(['microphone']) + + mic = Mic(pm) + mic.micd_thread() + + +if __name__ == "__main__": + main() diff --git a/tools/replay/replay.h b/tools/replay/replay.h index 88c285125..2c68443df 100644 --- a/tools/replay/replay.h +++ b/tools/replay/replay.h @@ -10,7 +10,7 @@ const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"; // one segment uses about 100M of memory -constexpr int FORWARD_FETCH_SEGS = 3; +constexpr int MIN_SEGMENTS_CACHE = 5; enum REPLAY_FLAGS { REPLAY_FLAG_NONE = 0x0000, @@ -58,7 +58,7 @@ public: event_filter = filter; } inline int segmentCacheLimit() const { return segment_cache_limit; } - inline void setSegmentCacheLimit(int n) { segment_cache_limit = std::max(3, n); } + inline void setSegmentCacheLimit(int n) { segment_cache_limit = std::max(MIN_SEGMENTS_CACHE, n); } inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; } inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; } inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } @@ -79,6 +79,7 @@ public: signals: void streamStarted(); void segmentsMerged(); + void seekedTo(double sec); protected slots: void segmentLoadFinished(bool success); @@ -133,5 +134,5 @@ protected: float speed_ = 1.0; replayEventFilter event_filter = nullptr; void *filter_opaque = nullptr; - int segment_cache_limit = 3; + int segment_cache_limit = MIN_SEGMENTS_CACHE; };