mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-09-29 19:03:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot 2023-03-29T09:55:29 for EON/C2

Browse Source 
version: dragonpilot v0.9.2 beta for EON/C2
date: 2023-03-29T09:55:29
dp-dev(priv2) master commit: 7a10a5f475f257bfcaf0f300d0441aef80be52d8
This commit is contained in:
Dragonpilot Team
2023-03-29 09:53:55 +00:00
parent ea800c8f74
commit eeb43a5107
132 changed files with 6269 additions and 3943 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOGS.md
+14
View File
@@ -1,3 +1,17 @@
dragonpilot [2023.3.27]
=======================
* Synced with openpilot master 2023.03.26 commits.
* comma 0.9.2 release
* Draw MPC path instead of model predicted path, this is a more accurate representation of what the car will do.
* DP Highlight
* Dynamic Rainbow mode. (fancy lol)
* Different color when Always On Lateral is enabled.
* RoadEdge detection has it's own toggle now.
* IMPROVEMENT: Better TSS2 Long and DF tune. @cgw1968-5779
* NEW: Alternative Lateral Controller
* Ability to switch to another lateral controller when SET speed is higher than configure speed.
* E.g. Use Torque + Laneless when belows 80kph and use PID + Laneline when above.
dragonpilot [2023.2.15]
=======================
* Synced with openpilot master 2022.12.14 commits.
CHANGELOGS_c2.md
+11 -7
View File
@@ -1,16 +1,20 @@
dragonpilot [Latest] - EON/C2 Release
========================
* Synced with openpilot master 2023.03.08 commits.
* Synced with openpilot master 2023.03.26 commits.
* DP Highlight
* Refactored cameraview widget.
* Updated VisionIPC to match current version.
* Rainbow mode. (fancy lol)
* Different color when Always On Lateral is enabled.
* IMPROVEMENT: Better TSS2 Long and DF tune. @cgw1968-5779
* NEW: Rainbow mode. (fancy lol)
* NEW: Local Trip counter.
* NEW: Alternative Lateral Controller
* Ability to switch to another lateral controller when SET speed is higher than configure speed.
* E.g. Use Torque + Laneless when belows 80kph and use PID + Laneline when above.
* DM reverted back to 0.8.13 model and algorithm.
* TWEAK: **10s Lat MPC is removed due to poor performance on EON/C2**
* TWEAK: Refactored cameraview widget.
* TWEAK: Different color when Always On Lateral is enabled.
* TWEAK: Better TSS2 Long and DF tune. @cgw1968-5779
* TWEAK: e2e Lateral will display lanelines in white.
* TWEAK: Updated VisionIPC to match current version.
* TWEAK: DM reverted to 0.8.13 model and algorithm.
* FIXED: Fixed Reverse message not showing issue.
dragonpilot 2023.02.08 - EON/C2 Release
========================
README.md
+4 -3
View File
@@ -121,7 +121,10 @@ Directory Structure
├── clocksd # Broadcasts current time
├── hardware # Hardware abstraction classes
├── logcatd # systemd journal as a service
└── proclogd # Logs information from /proc
├── loggerd # Logger and uploader of car data
├── proclogd # Logs information from /proc
├── sensord # IMU interface code
└── ubloxd # u-blox GNSS module interface code
└── selfdrive # Code needed to drive the car
├── assets # Fonts, images, and sounds for UI
├── athena # Allows communication with the app
@@ -130,12 +133,10 @@ Directory Structure
├── controls # Planning and controls
├── debug # Tools to help you debug and do car ports
├── locationd # Precise localization and vehicle parameter estimation
├── loggerd # Logger and uploader of car data
├── manager # Daemon that starts/stops all other daemons as needed
├── modeld # Driving and monitoring model runners
├── monitoring # Daemon to determine driver attention
├── navd # Turn-by-turn navigation
├── sensord # IMU interface code
├── test # Unit tests, system tests, and a car simulator
└── ui # The UI
RELEASES.md
+1
View File
@@ -1,6 +1,7 @@
Version 0.9.2 (2023-03-XX)
========================
* Draw MPC path instead of model predicted path, this is a more accurate representation of what the car will do.
* Buick LaCrosse 2017-19 support thanks to koch-cf!
* Škoda Fabia 2022-23 support thanks to jyoung8607!
Version 0.9.1 (2023-02-28)
cereal/car.capnp
+5 -1
View File
@@ -118,6 +118,8 @@ struct CarEvent @0x9b1657f34caf3ad3 {
#dp
speedLimitActive @116;
speedLimitValueChange @117;
leadMovingAlertSilent @118;
leadMovingAlert @119;
radarCanErrorDEPRECATED @15;
communityFeatureDisallowedDEPRECATED @62;
@@ -638,6 +640,7 @@ struct CarParams {
brand @6 :Text;
bus @7 :UInt8;
logging @8 :Bool;
obdMultiplexing @9 :Bool;
}
enum Ecu {
@@ -656,6 +659,7 @@ struct CarParams {
shiftByWire @16;
adas @19;
cornerRadar @21;
hvac @20;
# Toyota only
dsu @6;
@@ -669,7 +673,7 @@ struct CarParams {
hcp @18; # Hybrid Control Processor
# Hyundai only
vcu @20; # Vehicle (Motor) Control Unit
parking @22; # ADAS parking ECU
debug @17;
}
cereal/dp.capnp
+1
View File
@@ -53,4 +53,5 @@ struct DragonConf {
dpE2EConditionalAdaptFp @45 :Bool;
dpE2EConditionalAdaptAp @46 :Bool;
dpE2EConditionalVoacc @47 :Bool;
dpLongLeadMovingAlert @48 :Int8;
}
cereal/libcereal_shared.so
BIN
View File
Binary file not shown.
cereal/log.capnp
+44 -7
View File
@@ -1041,6 +1041,7 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
}
}
struct UiPlan {
frameId @2 :UInt32;
position @0 :XYZTData;
accel @1 :List(Float32);
}
@@ -1225,13 +1226,22 @@ struct GnssMeasurements {
gpsTimeOfWeek @2 :Float64;
correctedMeasurements @3 :List(CorrectedMeasurement);
ephemerisStatuses @9 :List(EphemerisStatus);
kalmanPositionECEF @4 :LiveLocationKalman.Measurement;
kalmanVelocityECEF @5 :LiveLocationKalman.Measurement;
positionECEF @6 :LiveLocationKalman.Measurement;
velocityECEF @7 :LiveLocationKalman.Measurement;
timeToFirstFix @8 :Float32;
# Todo sync this with timing pulse of ublox
struct EphemerisStatus {
constellationId @0 :ConstellationId;
svId @1 :UInt8;
type @2 :EphemerisType;
source @3 :EphemerisSource;
}
struct CorrectedMeasurement {
constellationId @0 :ConstellationId;
svId @1 :UInt8;
@@ -1244,11 +1254,11 @@ struct GnssMeasurements {
# Satellite position and velocity [x,y,z]
satPos @7 :List(Float64);
satVel @8 :List(Float64);
ephemerisSource @9 :EphemerisSource;
ephemerisSourceDEPRECATED @9 :EphemerisSourceDEPRECATED;
}
struct EphemerisSource {
type @0 :EphemerisSourceType;
struct EphemerisSourceDEPRECATED {
type @0 :EphemerisType;
# first epoch in file:
gpsWeek @1 :Int16; # -1 if Nav
gpsTimeOfWeek @2 :Int32; # -1 if Nav. Integer for seconds is good enough for logs.
@@ -1265,7 +1275,7 @@ struct GnssMeasurements {
glonass @6;
}
enum EphemerisSourceType {
enum EphemerisType {
nav @0;
# Different ultra-rapid files:
nasaUltraRapid @1;
@@ -1273,6 +1283,13 @@ struct GnssMeasurements {
# dp - eon/c2 need this renamed
qcompoly @3;
}
enum EphemerisSource {
gnssChip @0;
internet @1;
cache @2;
unknown @3;
}
}
struct UbloxGnss {
@@ -1283,6 +1300,19 @@ struct UbloxGnss {
hwStatus @3 :HwStatus;
hwStatus2 @4 :HwStatus2;
glonassEphemeris @5 :GlonassEphemeris;
satReport @6 :SatReport;
}
struct SatReport {
#received time of week in gps time in seconds and gps week
iTow @0 :UInt32;
svs @1 :List(SatInfo);
struct SatInfo {
svId @0 :UInt8;
gnssId @1 :UInt8;
flagsBitfield @2 :UInt32;
}
}
struct MeasurementReport {
@@ -1376,7 +1406,7 @@ struct UbloxGnss {
iDot @26 :Float64;
codesL2 @27 :Float64;
gpsWeek @28 :Float64;
gpsWeekDEPRECATED @28 :Float64;
l2 @29 :Float64;
svAcc @30 :Float64;
@@ -1394,6 +1424,8 @@ struct UbloxGnss {
ionoBeta @39 :List(Float64);
towCount @40 :UInt32;
toeWeek @41 :UInt16;
tocWeek @42 :UInt16;
}
struct IonoData {
@@ -1472,7 +1504,7 @@ struct UbloxGnss {
age @17 :UInt8;
svHealth @18 :UInt8;
tk @19 :UInt16;
tkDEPRECATED @19 :UInt16;
tb @20 :UInt16;
tauN @21 :Float64;
@@ -1484,7 +1516,12 @@ struct UbloxGnss {
p3 @26 :UInt8;
p4 @27 :UInt8;
freqNum @28 :UInt32;
freqNumDEPRECATED @28 :UInt32;
n4 @29 :UInt8;
nt @30 :UInt16;
freqNum @31 :Int16;
tkSeconds @32 :UInt32;
}
}
cereal/visionipc/visionipc.pxd
+3
View File
@@ -3,6 +3,7 @@
from libcpp.string cimport string
from libcpp.vector cimport vector
from libcpp.set cimport set
from libc.stdint cimport uint32_t, uint64_t
from libcpp cimport bool
@@ -40,3 +41,5 @@ cdef extern from "cereal/visionipc/visionipc_client.h":
VisionBuf * recv(VisionIpcBufExtra *, int)
bool connect(bool)
bool is_connected()
@staticmethod
set[VisionStreamType] getAvailableStreams(string, bool)
cereal/visionipc/visionipc_pyx.cpp
+1321 -956
View File
File diff suppressed because it is too large Load Diff
cereal/visionipc/visionipc_pyx.so
BIN
View File
Binary file not shown.
common/dp_conf.py
+4
View File
@@ -197,6 +197,10 @@ confs = [
# {'name': 'dp_ftpd', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
{'name': 'dp_lateral_version', 'default': 0, 'min': 0, 'max': 2, 'type': 'UInt8', 'conf_type': ['param']},
{'name': 'dp_lateral_steer_rate_cost', 'default': 1.0, 'min': 0.5, 'max': 2.0, 'type': 'Float32', 'conf_type': ['param']},
{'name': 'dp_upload_ignored', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
{'name': 'local_trip_count_total', 'default': 0.0, 'type': 'Float32', 'conf_type': ['param']},
{'name': 'local_trip_meter_total', 'default': 0.0, 'type': 'Float32', 'conf_type': ['param']},
{'name': 'local_trip_min_total', 'default': 0.0, 'type': 'Float32', 'conf_type': ['param']},
]
def get_definition(name):
common/params_pyx.so
BIN
View File
Binary file not shown.
common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "2023.03.27"
#define COMMA_VERSION "2023.03.29"
docs/CARS.md
+6 -5
View File
@@ -4,7 +4,7 @@
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.
# 238 Supported Cars
# 239 Supported Cars
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|Video|
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
@@ -17,6 +17,7 @@ A supported vehicle is one that just works when you install a comma three. All s
|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=Q3 2019-23">J533</a>||
|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=RS3 2018">J533</a>||
|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=S3 2015-17">J533</a>||
|Buick|LaCrosse 2017-19[<sup>3</sup>](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Buick&model=LaCrosse 2017-19">OBD-II</a>||
|Cadillac|Escalade 2017[<sup>3</sup>](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Cadillac&model=Escalade 2017">OBD-II</a>||
|Cadillac|Escalade ESV 2016[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Cadillac&model=Escalade ESV 2016">OBD-II</a>||
|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Bolt EUV 2022-23">GM</a>|<a href="https://youtu.be/xvwzGMUA210" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
@@ -116,7 +117,7 @@ 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[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Seltos 2021">Hyundai A</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)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2018">Hyundai C</a>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2019">Hyundai E</a>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Kia|Sorento 2022-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2022-23">Hyundai K</a>||
|Kia|Sorento 2021-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2021-23">Hyundai K</a>||
|Kia|Sorento Plug-in Hybrid 2022-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento Plug-in Hybrid 2022-23">Hyundai A</a>||
|Kia|Sportage 2023[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sportage 2023">Hyundai N</a>||
|Kia|Sportage Hybrid 2023[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sportage Hybrid 2023">Hyundai N</a>||
@@ -125,14 +126,14 @@ A supported vehicle is one that just works when you install a comma three. All s
|Kia|Telluride 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Telluride 2020-22">Hyundai H</a>||
|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=CT Hybrid 2017-18">Toyota</a>||
|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES 2019-22">Toyota</a>||
|Lexus|ES Hybrid 2017-18|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES Hybrid 2017-18">Toyota</a>||
|Lexus|ES Hybrid 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES Hybrid 2017-18">Toyota</a>||
|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES Hybrid 2019-23">Toyota</a>|<a href="https://youtu.be/BZ29osRVJeg?t=12" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=IS 2017-19">Toyota</a>||
|Lexus|NX 2018-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX 2018-19">Toyota</a>||
|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX 2020-21">Toyota</a>||
|Lexus|NX Hybrid 2018-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX Hybrid 2018-19">Toyota</a>||
|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX Hybrid 2020-21">Toyota</a>||
|Lexus|RC 2017-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RC 2017-20">Toyota</a>||
|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RC 2018-20">Toyota</a>||
|Lexus|RX 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2016">Toyota</a>||
|Lexus|RX 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2017-19">Toyota</a>||
|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2020-22">Toyota</a>||
@@ -148,7 +149,7 @@ A supported vehicle is one that just works when you install a comma three. All s
|Nissan|Leaf 2018-22|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Leaf 2018-22">Nissan A</a>|<a href="https://youtu.be/vaMbtAh_0cY" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Rogue 2018-20">Nissan A</a>||
|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=X-Trail 2017">Nissan A</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)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ram&model=1500 2019-22">Ram</a>||
|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ram&model=1500 2019-23">Ram</a>||
|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=SEAT&model=Ateca 2018">J533</a>||
|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=SEAT&model=Leon 2014-20">J533</a>||
|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Ascent 2019-21">Subaru A</a>||
launch_chffrplus.sh
-12
View File
@@ -221,18 +221,6 @@ function agnos_init {
fi
$DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST
fi
#dp: change splash logo
if [ -f "/usr/comma/.dp_splash" ]; then
echo "DP splash exists."
else
echo "DP splash not deployed yet"
sudo mount -o rw,remount /
sudo cp /data/openpilot/selfdrive/dragonpilot/bg.jpg /usr/comma/bg.jpg
sudo touch /usr/comma/.dp_splash
sudo mount -o ro,remount /
sudo reboot
fi
}
function launch {
opendbc/can/libdbc.so
BIN
View File
Binary file not shown.
opendbc/can/parser_pyx.so
BIN
View File
Binary file not shown.
opendbc/generator/toyota/_toyota_2017.dbc
+1
View File
@@ -82,6 +82,7 @@ BO_ 466 PCM_CRUISE: 8 XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
BO_ 467 PCM_CRUISE_2: 8 XXX
SG_ BRAKE_PRESSED : 3|1@0+ (1,0) [0|1] "" XXX
SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX
SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX
SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX
opendbc/generator/toyota/toyota_new_mc_pt.dbc
+3
View File
@@ -11,6 +11,9 @@ BO_ 610 EPS_STATUS: 5 EPS
SG_ TYPE : 24|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX
BO_ 1178 BRAKE_RELATED: 8 XXX
SG_ BRAKE_PRESSED : 48|1@0+ (1,0) [0|1] "" XXX
CM_ SG_ 548 BRAKE_PRESSURE "seems prop to pedal force";
CM_ SG_ 548 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";
opendbc/toyota_new_mc_pt_generated.dbc
+4
View File
@@ -132,6 +132,7 @@ BO_ 466 PCM_CRUISE: 8 XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
BO_ 467 PCM_CRUISE_2: 8 XXX
SG_ BRAKE_PRESSED : 3|1@0+ (1,0) [0|1] "" XXX
SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX
SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX
SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX
@@ -552,6 +553,9 @@ BO_ 610 EPS_STATUS: 5 EPS
SG_ TYPE : 24|1@0+ (1,0) [0|1] "" XXX
SG_ CHECKSUM : 39|8@0+ (1,0) [0|255] "" XXX
BO_ 1178 BRAKE_RELATED: 8 XXX
SG_ BRAKE_PRESSED : 48|1@0+ (1,0) [0|1] "" XXX
CM_ SG_ 548 BRAKE_PRESSURE "seems prop to pedal force";
CM_ SG_ 548 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";
opendbc/toyota_nodsu_pt_generated.dbc
+1
View File
@@ -133,6 +133,7 @@ BO_ 466 PCM_CRUISE: 8 XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
BO_ 467 PCM_CRUISE_2: 8 XXX
SG_ BRAKE_PRESSED : 3|1@0+ (1,0) [0|1] "" XXX
SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX
SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX
SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX
opendbc/toyota_tnga_k_pt_generated.dbc
+1
View File
@@ -132,6 +132,7 @@ BO_ 466 PCM_CRUISE: 8 XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
BO_ 467 PCM_CRUISE_2: 8 XXX
SG_ BRAKE_PRESSED : 3|1@0+ (1,0) [0|1] "" XXX
SG_ PCM_FOLLOW_DISTANCE : 12|2@0+ (1,0) [0|3] "" XXX
SG_ LOW_SPEED_LOCKOUT : 14|2@0+ (1,0) [0|3] "" XXX
SG_ MAIN_ON : 15|1@0+ (1,0) [0|1] "" XXX
opendbc/vw_golf_mk4.dbc
+37 -6
View File
@@ -1086,10 +1086,10 @@ BO_ 872 ACC_System: 8 XXX
SG_ ACS_Sollbeschl : 24|11@1+ (0.005,-7.22) [-7.22|3.005] "Unit_MeterPerSeconSquar" XXX
SG_ ACS_Anhaltewunsch : 38|1@1+ (1,0) [0|1] "" XXX
SG_ ACS_Fehler : 39|1@1+ (1,0) [0|1] "" XXX
SG_ ACS_zul_Regelabw : 40|8@1+ (1,0.005) [0|1.265] "Unit_MeterPerSeconSquar" XXX
SG_ ACS_max_AendGrad : 48|8@1+ (1,0.02) [0.02|5.06] "Unit_MeterPerSeconSquar" XXX
SG_ ACS_zul_Regelabw : 40|8@1+ (0.005,0) [0|1.265] "Unit_MeterPerSeconSquar" XXX
SG_ ACS_max_AendGrad : 48|8@1+ (0.02,0) [0.02|5.06] "Unit_MeterPerSeconSquar" XXX
BO_ 1386 ACC_GRA_Anziege: 8 XXX
BO_ 1386 ACC_GRA_Anzeige: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ ACA_StaACC : 8|3@1+ (1,0) [0|7] "" XXX
SG_ ACA_ID_StaACC : 11|5@1+ (1,0) [0|31] "" XXX
@@ -1100,7 +1100,7 @@ BO_ 1386 ACC_GRA_Anziege: 8 XXX
SG_ ACA_kmh_mph : 32|1@1+ (1,0) [0|1] "" XXX
SG_ ACA_Akustik1 : 33|1@1+ (1,0) [0|1] "" XXX
SG_ ACA_Akustik2 : 34|1@1+ (1,0) [0|1] "" XXX
SG_ ACA_PrioDisp : 35|1@1+ (1,0) [0|1] "" XXX
SG_ ACA_PrioDisp : 35|2@1+ (1,0) [0|3] "" XXX
SG_ ACA_gemZeitl : 40|4@1+ (1,0) [0|15] "" XXX
SG_ ACA_ACC_Verz : 44|1@1+ (1,0) [0|1] "" XXX
SG_ ACA_StaGRA : 48|3@1+ (1,0) [0|7] "" XXX
@@ -1153,8 +1153,8 @@ BO_ 644 Motor_Bremse: 6 XXX
SG_ MOB_COUNTER : 8|4@1+ (1,0) [0|15] "" Bremse_MK25AESP
SG_ TSK_v_Begrenzung_aktiv : 15|1@0+ (1,0) [0|1] "" XXX
SG_ TSK_ax_Getriebe_01 : 40|8@1+ (0.048,0) [0|255] "m/s2" XXX
SG_ MOB_Bremsstgr : 16|11@1+ (0.76,0) [0|100] "Unit_PerCent" Vector__XXX
SG_ MOB_Bremsmom : 27|13@1+ (1,0) [0|8190] "Unit_NewtoMeter" Bremse_MK25AESP
SG_ MOB_Bremsstgr : 16|11@1+ (0.048852,0) [0|100] "Unit_PerCent" Vector__XXX
SG_ MOB_Bremsmom : 27|13@1+ (4,0) [0|32760] "Unit_NewtoMeter" Bremse_MK25AESP
BO_ 870 AWV: 5 XXX
SG_ AWV_2_Gurtstraffer : 39|1@1+ (1,0) [0|1] "" Bremsbooster
@@ -1531,5 +1531,36 @@ CM_ SG_ 1470 XX_DLCORTLC2 "Might be DLC or TLC, might have wrong size";
CM_ SG_ 1550 MFA_v_Signal_02 "0=km/h, 1=mph";
VAL_ 872 ACS_Sta_ADR 2 "ADR_passiv" 0 "ADR_nicht_aktiv" 1 "ADR_aktiv" 3 "irrev_Fehler" ;
VAL_ 872 ACS_ADR_Schub 1 "Verz_begr_auf_Schub" 0 "Verz_nicht_begr_auf_Schub" ;
VAL_ 872 ACS_Schubabsch 1 "SA_nicht_zulaessig" 0 "SA_zulaessig" ;
VAL_ 872 ACS_StSt_Info 3 "Systemfehler" 0 "Motorlauf_nn" 1 "Stoppverbot_Motoranlauf_nn" 2 "Motoranlauf_notwendig" ;
VAL_ 872 ACS_MomEingriff 1 "MomEingr_verhindern" 0 "keine_Beeinfl_MomEingr_Mot" ;
VAL_ 872 ACS_Typ_ACC 0 "Basis_ACC" 1 "ACC_mit_FollowToStop" 3 "frei" 2 "frei" ;
VAL_ 872 ACS_FreigSollB 0 "Sollbeschl_nicht_freigeg" 1 "Sollbeschl_freigeg" ;
VAL_ 872 ACS_Sollbeschl 2046 "ADR_nicht_aktiv" 2047 "Fehler" ;
VAL_ 872 ACS_Anhaltewunsch 0 "kein_Haltewunsch" 1 "Fzg_haelt_an" ;
VAL_ 872 ACS_Fehler 1 "Fehlerspeichereintrag" 0 "kein_Fehlerspeichereintrag" ;
VAL_ 872 ACS_zul_Regelabw 254 "ADR_nicht_aktiv" 255 "Fehler" ;
VAL_ 872 ACS_max_AendGrad 254 "Neutralwert" 0 "Neutralwert" 255 "Fehler" ;
VAL_ 978 LH2_Sta_HCA 0 "disabled" 1 "initializing" 2 "fault" 3 "ready" 4 "rejected" 5 "active";
VAL_ 1088 Waehlhebelposition__Getriebe_1_ 8 "P" 7 "R" 6 "N" 5 "D" 9 "U" 12 "S" 14 "T" 10 "T" 11 "T";
VAL_ 1386 ACA_StaACC 6 "ACC_rev_aus" 0 "Hauptschalter_aus" 4 "ACC_im_Hintergrund" 3 "ACC_aktiv" 1 "Reserve" 2 "ACC_passiv" 7 "ACC_irrev_aus" 5 "frei" ;
VAL_ 1386 ACA_ID_StaACC 0 "keine_Anzeige" ;
VAL_ 1386 ACA_Fahrerhinw 1 "Ein" 0 "Aus" ;
VAL_ 1386 ACA_AnzDisplay 1 "Anzeige_erw" 0 "Anzeige_nicht_erw" ;
VAL_ 1386 ACA_Zeitluecke 3 "Zeitluecke3" 10 "Zeitluecke10" 4 "Zeitluecke4" 14 "Zeitluecke14" 11 "Zeitluecke11" 2 "Zeitluecke2" 13 "Zeitluecke13" 9 "Zeitluecke9" 1 "Zeitluecke1" 8 "Zeitluecke8" 5 "Zeitluecke5" 15 "Zeitluecke15" 0 "nicht_definiert" 12 "Zeitluecke12" 6 "Zeitluecke6" 7 "Zeitluecke7" ;
VAL_ 1386 ACA_V_Wunsch 255 "kein_Wert_im_Speicher" ;
VAL_ 1386 ACA_kmh_mph 0 "km_h" 1 "mph" ;
VAL_ 1386 ACA_Akustik1 0 "kein_Gong" 1 "Gong" ;
VAL_ 1386 ACA_Akustik2 0 "kein_Summer" 1 "Summer" ;
VAL_ 1386 ACA_PrioDisp 1 "mittlere_Prio" 3 "keine_Anzeige_Anf" 0 "hohe_Prio" 2 "niedrige_Prio" ;
VAL_ 1386 ACA_gemZeitl 6 "Zeitluecke6" 2 "Zeitluecke2" 7 "Zeitluecke7" 13 "Zeitluecke13" 11 "Zeitluecke11" 4 "Zeitluecke4" 8 "Zeitluecke8" 12 "Zeitluecke12" 10 "Zeitluecke10" 0 "Kein_Objekt_erfasst" 1 "Zeitluecke1" 3 "Zeitluecke3" 9 "Zeitluecke9" 15 "Zeitluecke15" 14 "Zeitluecke14" 5 "Zeitluecke5" ;
VAL_ 1386 ACA_ACC_Verz 0 "ACC_verzoegert_nicht" 1 "ACC_verzoegert" ;
VAL_ 1386 ACA_StaGRA 3 "GRA_aktiv" 4 "GRA_uebertreten" 2 "GRA_passiv" 0 "Hauptschalter_aus" 6 "frei" 7 "GRA_Fehler" 1 "Reserve" 5 "frei" ;
VAL_ 1386 ACA_ID_StaGRA 0 "keine_Anzeige" ;
VAL_ 1386 ACA_Codierung 0 "ACC" 1 "GRA" ;
VAL_ 1386 ACA_Tachokranz 0 "nicht_beleuchtet" 1 "beleuchtet" ;
VAL_ 1386 ACA_Aend_Zeitluecke 1 "Anzeige_angef" 0 "keine_Anzeige" ;
panda/board/obj/bootstub.panda.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda.bin.signed
BIN
View File
Binary file not shown.
panda/python/__init__.py
+5
View File
@@ -201,6 +201,7 @@ class Panda:
# first byte is for EPS scaling factor
FLAG_TOYOTA_ALT_BRAKE = (1 << 8)
FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8)
FLAG_TOYOTA_LTA = (4 << 8)
FLAG_HONDA_ALT_BRAKE = 1
FLAG_HONDA_BOSCH_LONG = 2
@@ -404,6 +405,10 @@ class Panda:
if not enter_bootloader and reconnect:
self.reconnect()
@property
def connected(self) -> bool:
return self._handle_open
def reconnect(self):
if self._handle_open:
self.close()
rednose/helpers/ekf_sym_pyx.so
BIN
View File
Binary file not shown.
selfdrive/athena/registration.py
+1 -1
View File
@@ -100,7 +100,7 @@ def register(show_spinner=False) -> Optional[str]:
if dongle_id:
params.put("DongleId", dongle_id)
set_offroad_alert("Offroad_UnofficialHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC)
# set_offroad_alert("Offroad_UnofficialHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC)
return dongle_id
selfdrive/boardd/boardd
BIN
View File
Binary file not shown.
selfdrive/boardd/boardd_api_impl.so
BIN
View File
Binary file not shown.
selfdrive/boardd/tests/test_boardd_loopback.py
+1 -1
View File
@@ -51,7 +51,7 @@ class TestBoardd(unittest.TestCase):
cp.safetyConfigs = [safety_config]*num_pandas
params = Params()
params.put("CarVin", b"0"*17)
params.put_bool("FirmwareQueryDone", True)
params.put_bool("ControlsReady", True)
params.put("CarParams", cp.to_bytes())
selfdrive/car/car_helpers.py
+9 -6
View File
@@ -8,7 +8,7 @@ from common.basedir import BASEDIR
from selfdrive.car.interfaces import get_interface_attr
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
from selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
from selfdrive.car.fw_versions import disable_obd_multiplexing, get_fw_versions_ordered, match_fw_to_car, get_present_ecus
from selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing
from system.swaglog import cloudlog
import cereal.messaging as messaging
from selfdrive.car import gen_empty_fingerprint
@@ -85,6 +85,7 @@ def fingerprint(logcan, sendcan, num_pandas):
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
ecu_rx_addrs = set()
params = Params()
dp_car_assigned = Params().get('dp_car_assigned', encoding='utf8')
if not fixed_fingerprint and dp_car_assigned is not None:
@@ -95,7 +96,7 @@ def fingerprint(logcan, sendcan, num_pandas):
# Vin query only reliably works through OBDII
bus = 1
cached_params = Params().get("CarParamsCache")
cached_params = params.get("CarParamsCache")
if cached_params is not None:
cached_params = car.CarParams.from_bytes(cached_params)
if cached_params.carName == "mock":
@@ -108,6 +109,7 @@ def fingerprint(logcan, sendcan, num_pandas):
cached = True
else:
cloudlog.warning("Getting VIN & FW versions")
set_obd_multiplexing(params, True)
vin_rx_addr, vin = get_vin(logcan, sendcan, bus)
ecu_rx_addrs = get_present_ecus(logcan, sendcan, num_pandas=num_pandas)
car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas)
@@ -123,10 +125,11 @@ def fingerprint(logcan, sendcan, num_pandas):
cloudlog.event("Malformed VIN", vin=vin, error=True)
vin = VIN_UNKNOWN
cloudlog.warning("VIN %s", vin)
params = Params()
params.put("CarVin", vin)
disable_obd_multiplexing(params)
# disable OBD multiplexing for potential ECU knockouts
set_obd_multiplexing(params, False)
params.put_bool("FirmwareQueryDone", True)
finger = gen_empty_fingerprint()
candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
@@ -225,7 +228,7 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1):
candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas)
if candidate is None:
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
cloudlog.event("car doesn't match any fingerprints", fingerprints=fingerprints, error=True)
candidate = "mock"
y = threading.Thread(target=crash_log2, args=(fingerprints,car_fw,))
selfdrive/car/chrysler/values.py
+119 -37
View File
@@ -50,6 +50,7 @@ class CarControllerParams:
self.STEER_DELTA_DOWN = 3
self.STEER_MAX = 261 # higher than this faults the EPS
STEER_THRESHOLD = 120
RAM_DT = {CAR.RAM_1500, }
@@ -62,6 +63,7 @@ class ChryslerCarInfo(CarInfo):
package: str = "Adaptive Cruise Control (ACC)"
harness: Enum = Harness.fca
CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = {
CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017-18"),
CAR.PACIFICA_2018_HYBRID: None, # same platforms
@@ -73,10 +75,10 @@ CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = {
],
CAR.JEEP_CHEROKEE: ChryslerCarInfo("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk"),
CAR.JEEP_CHEROKEE_2019: ChryslerCarInfo("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4"),
CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-22", harness=Harness.ram),
CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-23", harness=Harness.ram),
CAR.RAM_HD: [
ChryslerCarInfo("Ram 2500 2020-22", harness=Harness.ram),
ChryslerCarInfo("Ram 3500 2020-22", harness=Harness.ram),
ChryslerCarInfo("Ram 3500 2019-22", harness=Harness.ram),
],
}
@@ -99,7 +101,7 @@ FINGERPRINTS = {
55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
},
{
55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
55: 8, 58: 6, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 956: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8
}],
CAR.PACIFICA_2020: [{
55: 8, 179: 8, 181: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 536: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 776: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 886: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1543: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 2015: 8, 2016: 8, 2017:8, 2024: 8, 2025: 8
@@ -180,79 +182,153 @@ FW_QUERY_CONFIG = FwQueryConfig(
)
FW_VERSIONS = {
CAR.RAM_1500: {
CAR.JEEP_CHEROKEE_2019: {
(Ecu.combinationMeter, 0x742, None): [
b'68294063AH',
b'68294063AG',
b'68434860AC',
b'68527375AD',
b'68453503AC',
b'68402971AD',
],
(Ecu.srs, 0x744, None): [
b'68441329AB',
b'68490898AA',
b'68428609AB',
b'68500728AA',
b'68355363AB',
],
(Ecu.abs, 0x747, None): [
b'68432418AD',
b'68432418AB',
b'68436004AE',
b'68438454AD',
b'68436004AD',
b'68535469AB',
b'68438454AC',
b'68408639AD',
],
(Ecu.fwdRadar, 0x753, None): [
b'68320950AL',
b'68320950AJ',
b'68454268AB',
b'68475160AG',
b'04672892AB',
b'68475160AE',
b'68456722AC',
],
(Ecu.eps, 0x75A, None): [
b'68273275AG',
b'68469901AA',
b'68552788AA',
b'68453431AA',
],
(Ecu.engine, 0x7e0, None): [
b'68448163AJ',
b'68500630AD',
b'68539650AD',
b'05035674AB ',
],
(Ecu.transmission, 0x7e1, None): [
b'05035707AA',
],
},
CAR.RAM_1500: {
(Ecu.combinationMeter, 0x742, None): [
b'68294051AG',
b'68294051AI',
b'68294052AG',
b'68294063AG',
b'68294063AH',
b'68294063AI',
b'68434846AC',
b'68434858AC',
b'68434860AC',
b'68453503AC',
b'68453505AC',
b'68453511AC',
b'68453513AD',
b'68453514AD',
b'68510283AG',
b'68527375AD',
],
(Ecu.srs, 0x744, None): [
b'68428609AB',
b'68441329AB',
b'68473844AB',
b'68490898AA',
b'68500728AA',
b'68615033AA',
],
(Ecu.abs, 0x747, None): [
b'68292406AH',
b'68432418AB',
b'68432418AD',
b'68436004AD',
b'68436004AE',
b'68438454AC',
b'68438454AD',
b'68438456AE',
b'68438456AF',
b'68535469AB',
b'68535470AC',
b'68586307AB',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672892AB',
b'04672932AB',
b'68320950AH',
b'68320950AI',
b'68320950AJ',
b'68320950AL',
b'68320950AM',
b'68454268AB',
b'68475160AE',
b'68475160AF',
b'68475160AG',
],
(Ecu.eps, 0x75A, None): [
b'68273275AF',
b'68273275AG',
b'68312176AE',
b'68312176AG',
b'68440789AC',
b'68466110AB',
b'68469901AA',
b'68522583AB',
b'68522585AB',
b'68552788AA',
b'68552790AA',
b'68585112AB',
],
(Ecu.engine, 0x7e0, None): [
b'05036065AE ',
b'05036066AE ',
b'68378701AI ',
b'68378758AM ',
b'68448163AJ',
b'68448165AK',
b'68500630AD',
b'68500630AE',
b'68539650AD',
],
(Ecu.transmission, 0x7e1, None): [
b'68360078AL',
b'68384328AD',
b'68360085AL',
b'68360080AM',
b'68360081AM',
b'68502994AD',
b'68360085AL',
b'68384328AD',
b'68384332AD',
b'68445533AB',
b'68540431AB',
b'68484467AC',
b'68502994AD',
b'68540431AB',
],
},
CAR.RAM_HD: {
(Ecu.combinationMeter, 0x742, None): [
b'68361606AH',
b'68437735AC',
b'68492693AD',
b'68525485AB',
b'68525487AB',
b'68525498AB',
],
(Ecu.srs, 0x744, None): [
b'68399794AC',
b'68428503AA',
b'68428505AA',
b'68428507AA',
],
(Ecu.abs, 0x747, None): [
b'68334977AH',
b'68455481AC',
b'68504022AA',
b'68504022AB',
b'68530686AB',
b'68504022AC',
b'68530686AB',
b'68530686AC',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672895AB',
b'56029827AG',
b'56029827AH',
b'68462657AE',
b'68484694AD',
b'68484694AE',
],
(Ecu.eps, 0x761, None): [
@@ -260,7 +336,13 @@ FW_VERSIONS = {
b'68507906AB',
],
(Ecu.engine, 0x7e0, None): [
b'52370131AF',
b'52370231AF',
b'52370231AG',
b'52370931CT',
b'52401032AE',
b'52421132AF',
b'68527616AD ',
b'M2370131MB',
b'M2421132MB',
],
selfdrive/car/ecu_addrs.py
+7 -5
View File
@@ -9,6 +9,8 @@ from selfdrive.car import make_can_msg
from selfdrive.boardd.boardd import can_list_to_can_capnp
from system.swaglog import cloudlog
EcuAddrBusType = Tuple[int, Optional[int], int]
def make_tester_present_msg(addr, bus, subaddr=None):
dat = [0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0]
@@ -33,16 +35,16 @@ def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subadd
return False
def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[Tuple[int, Optional[int], int]]:
def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[EcuAddrBusType]:
addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)]
queries: Set[Tuple[int, Optional[int], int]] = {(addr, None, bus) for addr in addr_list}
queries: Set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list}
responses = queries
return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug)
def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[Tuple[int, Optional[int], int]],
responses: Set[Tuple[int, Optional[int], int]], timeout: float = 1, debug: bool = False) -> Set[Tuple[int, Optional[int], int]]:
ecu_responses: Set[Tuple[int, Optional[int], int]] = set() # set((addr, subaddr, bus),)
def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[EcuAddrBusType],
responses: Set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> Set[EcuAddrBusType]:
ecu_responses: Set[EcuAddrBusType] = set() # set((addr, subaddr, bus),)
try:
msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries]
selfdrive/car/fw_query_definitions.py
+12 -2
View File
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
import capnp
import copy
from dataclasses import dataclass, field
import struct
from typing import Dict, List, Optional, Tuple
@@ -57,10 +58,12 @@ class Request:
whitelist_ecus: List[int] = field(default_factory=list)
rx_offset: int = 0x8
bus: int = 1
# Whether this query should be run on the first auxiliary panda (CAN FD cars for example)
auxiliary: bool = False
# FW responses from these queries will not be used for fingerprinting
logging: bool = False
# These requests are done once OBD multiplexing is disabled, after all others
non_obd: bool = False
# boardd toggles OBD multiplexing on/off as needed
obd_multiplexing: bool = True
@dataclass
@@ -71,3 +74,10 @@ class FwQueryConfig:
non_essential_ecus: Dict[capnp.lib.capnp._EnumModule, List[str]] = field(default_factory=dict)
# Ecus added for data collection, not to be fingerprinted on
extra_ecus: List[Tuple[capnp.lib.capnp._EnumModule, int, Optional[int]]] = field(default_factory=list)
def __post_init__(self):
for i in range(len(self.requests)):
if self.requests[i].auxiliary:
new_request = copy.deepcopy(self.requests[i])
new_request.bus += 4
self.requests.append(new_request)
selfdrive/car/fw_versions.py
+43 -47
View File
@@ -1,12 +1,12 @@
#!/usr/bin/env python3
from collections import defaultdict
from typing import Any, Optional, Set, Tuple
from typing import Any, Dict, List, Set
from tqdm import tqdm
import panda.python.uds as uds
from cereal import car
from common.params import Params
from selfdrive.car.ecu_addrs import get_ecu_addrs
from selfdrive.car.ecu_addrs import EcuAddrBusType, get_ecu_addrs
from selfdrive.car.interfaces import get_interface_attr
from selfdrive.car.fingerprints import FW_VERSIONS
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
@@ -19,7 +19,7 @@ FW_QUERY_CONFIGS = get_interface_attr('FW_QUERY_CONFIG', ignore_none=True)
VERSIONS = get_interface_attr('FW_VERSIONS', ignore_none=True)
MODEL_TO_BRAND = {c: b for b, e in VERSIONS.items() for c in e}
REQUESTS = [(brand, r) for brand, config in FW_QUERY_CONFIGS.items() for r in config.requests]
REQUESTS = [(brand, config, r) for brand, config in FW_QUERY_CONFIGS.items() for r in config.requests]
def chunks(l, n=128):
@@ -39,6 +39,8 @@ def build_fw_dict(fw_versions, filter_brand=None):
def get_brand_addrs():
brand_addrs = defaultdict(set)
for brand, cars in VERSIONS.items():
# Add ecus in database + extra ecus to match against
brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in FW_QUERY_CONFIGS[brand].extra_ecus}
for fw in cars.values():
brand_addrs[brand] |= {(addr, sub_addr) for _, addr, sub_addr in fw.keys()}
return brand_addrs
@@ -146,38 +148,43 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True):
return True, set()
def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[Tuple[int, Optional[int], int]]:
queries = list()
parallel_queries = list()
def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]:
params = Params()
# queries are split by OBD multiplexing mode
queries: Dict[bool, List[List[EcuAddrBusType]]] = {True: [], False: []}
parallel_queries: Dict[bool, List[EcuAddrBusType]] = {True: [], False: []}
responses = set()
for brand, r in REQUESTS:
for brand, config, r in REQUESTS:
# Skip query if no panda available
if r.bus > num_pandas * 4 - 1:
continue
for brand_versions in VERSIONS[brand].values():
for ecu_type, addr, sub_addr in brand_versions:
for ecu_type, addr, sub_addr in list(brand_versions) + config.extra_ecus:
# Only query ecus in whitelist if whitelist is not empty
if len(r.whitelist_ecus) == 0 or ecu_type in r.whitelist_ecus:
a = (addr, sub_addr, r.bus)
# Build set of queries
if sub_addr is None:
if a not in parallel_queries:
parallel_queries.append(a)
if a not in parallel_queries[r.obd_multiplexing]:
parallel_queries[r.obd_multiplexing].append(a)
else: # subaddresses must be queried one by one
if [a] not in queries:
queries.append([a])
if [a] not in queries[r.obd_multiplexing]:
queries[r.obd_multiplexing].append([a])
# Build set of expected responses to filter
response_addr = uds.get_rx_addr_for_tx_addr(addr, r.rx_offset)
responses.add((response_addr, sub_addr, r.bus))
queries.insert(0, parallel_queries)
for obd_multiplexing in queries:
queries[obd_multiplexing].insert(0, parallel_queries[obd_multiplexing])
ecu_responses = set()
for query in queries:
ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1))
for obd_multiplexing in queries:
set_obd_multiplexing(params, obd_multiplexing)
for query in queries[obd_multiplexing]:
ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1))
return ecu_responses
@@ -185,9 +192,9 @@ def get_brand_ecu_matches(ecu_rx_addrs):
"""Returns dictionary of brands and matches with ECUs in their FW versions"""
brand_addrs = get_brand_addrs()
brand_matches = {brand: set() for brand, _ in REQUESTS}
brand_matches = {brand: set() for brand, _, _ in REQUESTS}
brand_rx_offsets = set((brand, r.rx_offset) for brand, r in REQUESTS)
brand_rx_offsets = set((brand, r.rx_offset) for brand, _, r in REQUESTS)
for addr, sub_addr, _ in ecu_rx_addrs:
# Since we can't know what request an ecu responded to, add matches for all possible rx offsets
for brand, rx_offset in brand_rx_offsets:
@@ -198,13 +205,13 @@ def get_brand_ecu_matches(ecu_rx_addrs):
return brand_matches
def disable_obd_multiplexing(params):
if not params.get_bool("ObdMultiplexingDisabled"):
params.put_bool("FirmwareObdQueryDone", True)
cloudlog.warning("Waiting for OBD multiplexing to be disabled")
params.get_bool("ObdMultiplexingDisabled", block=True)
cloudlog.warning("OBD multiplexing disabled")
def set_obd_multiplexing(params: Params, obd_multiplexing: bool):
if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing:
cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}")
params.remove("ObdMultiplexingChanged")
params.put_bool("ObdMultiplexingEnabled", obd_multiplexing)
params.get_bool("ObdMultiplexingChanged", block=True)
cloudlog.warning("OBD multiplexing set successfully")
def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pandas=1, debug=False, progress=False):
@@ -212,7 +219,6 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand
all_car_fw = []
brand_matches = get_brand_ecu_matches(ecu_rx_addrs)
matched_brand: Optional[str] = None
for brand in sorted(brand_matches, key=lambda b: len(brand_matches[b]), reverse=True):
car_fw = get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress)
@@ -220,21 +226,14 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand
# Try to match using FW returned from this brand only
matches = match_fw_to_car_exact(build_fw_dict(car_fw))
if len(matches) == 1:
matched_brand = brand
break
disable_obd_multiplexing(Params())
# Do non-OBD queries for matched brand, or all if no match is found
for brand in FW_QUERY_CONFIGS.keys():
if brand == matched_brand or matched_brand is None:
all_car_fw.extend(get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, obd_multiplexed=False, debug=debug, progress=progress))
return all_car_fw
def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, obd_multiplexed=True, debug=False, progress=False):
def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, debug=False, progress=False):
versions = VERSIONS.copy()
params = Params()
# Each brand can define extra ECUs to query for data collection
for brand, config in FW_QUERY_CONFIGS.items():
@@ -250,19 +249,15 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1,
# ECUs using a subaddress need be queried one by one, the rest can be done in parallel
addrs = []
parallel_addrs = []
logging_addrs = []
ecu_types = {}
for brand, brand_versions in versions.items():
for candidate, ecu in brand_versions.items():
for ecu in brand_versions.values():
for ecu_type, addr, sub_addr in ecu.keys():
a = (brand, addr, sub_addr)
if a not in ecu_types:
ecu_types[a] = ecu_type
if a not in logging_addrs and candidate == "debug":
logging_addrs.append(a)
if sub_addr is None:
if a not in parallel_addrs:
parallel_addrs.append(a)
@@ -274,16 +269,17 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1,
# Get versions and build capnp list to put into CarParams
car_fw = []
requests = [(brand, r) for brand, r in REQUESTS if query_brand is None or brand == query_brand]
requests = [(brand, config, r) for brand, config, r in REQUESTS if query_brand is None or brand == query_brand]
for addr in tqdm(addrs, disable=not progress):
for addr_chunk in chunks(addr):
for brand, r in requests:
for brand, config, r in requests:
# Skip query if no panda available
if r.bus > num_pandas * 4 - 1:
continue
# Or if request is not designated for current multiplexing mode
elif r.non_obd == obd_multiplexed:
continue
# Toggle OBD multiplexing for each request
if r.bus % 4 == 1:
set_obd_multiplexing(params, r.obd_multiplexing)
try:
addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and
@@ -294,15 +290,15 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1,
for (tx_addr, sub_addr), version in query.get_data(timeout).items():
f = car.CarParams.CarFw.new_message()
ecu_key = (brand, tx_addr, sub_addr)
f.ecu = ecu_types.get(ecu_key, Ecu.unknown)
f.ecu = ecu_types.get((brand, tx_addr, sub_addr), Ecu.unknown)
f.fwVersion = version
f.address = tx_addr
f.responseAddress = uds.get_rx_addr_for_tx_addr(tx_addr, r.rx_offset)
f.request = r.request
f.brand = brand
f.bus = r.bus
f.logging = r.logging or ecu_key in logging_addrs
f.logging = r.logging or (f.ecu, tx_addr, sub_addr) in config.extra_ecus
f.obdMultiplexing = r.obd_multiplexing
if sub_addr is not None:
f.subAddress = sub_addr
selfdrive/car/gm/interface.py
+8 -3
View File
@@ -78,7 +78,6 @@ class CarInterface(CarInterfaceBase):
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.longitudinalActuatorDelayUpperBound = 0.5
if experimental_long:
ret.pcmCruise = False
@@ -116,6 +115,7 @@ class CarInterface(CarInterfaceBase):
ret.steerLimitTimer = 0.4
ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz
ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking
if candidate == CAR.VOLT:
ret.mass = 1607. + STD_CARGO_KG
@@ -151,7 +151,13 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 14.4 # end to end is 13.46
ret.centerToFront = ret.wheelbase * 0.4
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia()
ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking
elif candidate == CAR.BUICK_LACROSSE:
ret.mass = 1712. + STD_CARGO_KG
ret.wheelbase = 2.91
ret.steerRatio = 15.8
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BUICK_REGAL:
ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2
@@ -171,7 +177,6 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.95 # 116 inches in meters
ret.steerRatio = 17.3
ret.centerToFront = ret.wheelbase * 0.5
ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.ESCALADE_ESV:
selfdrive/car/gm/values.py
+7
View File
@@ -66,6 +66,7 @@ class CAR:
CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018"
MALIBU = "CHEVROLET MALIBU PREMIER 2017"
ACADIA = "GMC ACADIA DENALI 2018"
BUICK_LACROSSE = "BUICK LACROSSE 2017"
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
ESCALADE = "CADILLAC ESCALADE 2017"
ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016"
@@ -100,6 +101,7 @@ CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = {
CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"),
CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017"),
CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"),
CAR.BUICK_LACROSSE: GMCarInfo("Buick LaCrosse 2017-19", "Driver Confidence Package 2"),
CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"),
CAR.ESCALADE: GMCarInfo("Cadillac Escalade 2017", "Driver Assist Package"),
CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"),
@@ -160,6 +162,11 @@ FINGERPRINTS = {
{
170: 8, 171: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 209: 7, 211: 2, 241: 6, 288: 5, 289: 1, 290: 1, 298: 2, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 368: 8, 381: 2, 384: 8, 386: 5, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 3, 508: 8, 512: 3, 528: 4, 530: 8, 532: 6, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 761: 7, 810: 8, 821: 4, 823: 7, 832: 8, 840: 5, 842: 5, 844: 8, 853: 8, 866: 4, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7
}],
CAR.BUICK_LACROSSE: [
# LaCrosse Premium AWD 2017
{
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 5, 707: 8, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 872: 1, 882: 8, 890: 1, 892: 2, 893: 1, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1914: 7, 1916: 7, 1918: 7, 1919: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
}],
CAR.BUICK_REGAL : [
# Regal TourX Essence w/ ACC 2018
{
selfdrive/car/honda/values.py
+1 -1
View File
@@ -188,7 +188,7 @@ FW_QUERY_CONFIG = FwQueryConfig(
[StdQueries.UDS_VERSION_RESPONSE],
bus=1,
logging=True,
non_obd=True,
obd_multiplexing=False,
),
],
extra_ecus=[
selfdrive/car/hyundai/values.py
+42 -6
View File
@@ -224,7 +224,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
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_4TH_GEN: HyundaiCarInfo("Kia Sorento 2022-23", harness=Harness.hyundai_k),
CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", harness=Harness.hyundai_k),
CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", 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),
@@ -335,10 +335,15 @@ FINGERPRINTS = {
HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf100) # Long description
HYUNDAI_VERSION_REQUEST_ALT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(0xf110) # Alt long description
HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
p16(0xf100)
HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
FW_QUERY_CONFIG = FwQueryConfig(
@@ -355,22 +360,46 @@ FW_QUERY_CONFIG = FwQueryConfig(
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine, Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar],
),
# CAN-FD queries (camera)
# CAN-FD queries (from camera)
# TODO: combine shared whitelists with CAN requests
Request(
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar],
bus=4,
whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar, Ecu.hvac],
bus=0,
auxiliary=True,
),
Request(
[HYUNDAI_VERSION_REQUEST_LONG],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar],
bus=5,
whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar, Ecu.hvac],
bus=1,
auxiliary=True,
obd_multiplexing=False,
),
# CAN-FD debugging queries
Request(
[HYUNDAI_VERSION_REQUEST_ALT],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.parking],
bus=0,
auxiliary=True,
),
Request(
[HYUNDAI_VERSION_REQUEST_ALT],
[HYUNDAI_VERSION_RESPONSE],
whitelist_ecus=[Ecu.parking],
bus=1,
auxiliary=True,
obd_multiplexing=False,
),
],
extra_ecus=[
(Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms
(Ecu.parking, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms)
(Ecu.hvac, 0x7b3, None), # HVAC Control Assembly
(Ecu.cornerRadar, 0x7b7, None),
],
)
@@ -1552,6 +1581,8 @@ FW_VERSIONS = {
b'\xf1\x00CV1 MFC AT EUR LHD 1.00 1.06 99210-CV000 220328',
b'\xf1\x00CV1 MFC AT EUR RHD 1.00 1.00 99210-CV100 220630',
b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.00 99210-CV100 220630',
b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.04 99210-CV000 210823',
b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.06 99210-CV000 220328',
],
},
CAR.IONIQ_5: {
@@ -1562,6 +1593,7 @@ FW_VERSIONS = {
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.02 99211-GI010 211206',
b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813',
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614',
b'\xf1\x00NE1 MFC AT KOR LHD 1.00 1.05 99211-GI010 220614',
b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI010 211007',
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.01 99211-GI010 211007',
b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.02 99211-GI010 211206',
@@ -1628,6 +1660,7 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU100 211215',
b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.02 99211-CU000 211215',
b'\xf1\x00JW1 MFC AT USA LHD 1.00 1.03 99211-CU000 221118',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00JW1_ RDR ----- 1.00 1.00 99110-CU000 ',
@@ -1636,14 +1669,17 @@ FW_VERSIONS = {
CAR.KIA_SORENTO_4TH_GEN: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00MQ4 MFC AT USA LHD 1.00 1.05 99210-R5000 210623',
b'\xf1\x00MQ4 MFC AT USA LHD 1.00 1.03 99210-R5000 200903',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00MQ4_ SCC FHCUP 1.00 1.06 99110-P2000 ',
b'\xf1\x00MQ4_ SCC F-CUP 1.00 1.06 99110-P2000 ',
],
},
CAR.KIA_NIRO_HEV_2ND_GEN: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00SG2HMFC AT USA LHD 1.01 1.08 99211-AT000 220531',
b'\xf1\x00SG2HMFC AT USA LHD 1.01 1.09 99211-AT000 220801',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00SG2_ RDR ----- 1.00 1.01 99110-AT000 ',
selfdrive/car/subaru/values.py
+4
View File
@@ -273,9 +273,11 @@ FW_VERSIONS = {
b'\xa3 \031\024\000',
b'\xa3 \x14\x01',
b'\xf1\x00\xbb\r\x05',
b'\xa3 \x18&\x00',
],
(Ecu.eps, 0x746, None): [
b'\x8d\xc0\x04\x00',
b'\x8d\xc0\x00\x00',
],
(Ecu.fwdCamera, 0x787, None): [
b'\x00\x00e!\x1f@ \x11',
@@ -292,6 +294,7 @@ FW_VERSIONS = {
b'\xcb\"`p\a',
b'\xf1\x00\xa2\x10\n',
b'\xcf"`p\x07',
b'\xb6\xa2`A\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\032\xf6B0\000',
@@ -299,6 +302,7 @@ FW_VERSIONS = {
b'\032\xf6b`\000',
b'\x1a\xf6B`\x00',
b'\x1a\xf6b0\x00',
b'\x1a\xe6B1\x00',
],
},
CAR.FORESTER_PREGLOBAL: {
selfdrive/car/torque_data/override.yaml
+4
View File
@@ -7,6 +7,10 @@ NISSAN LEAF 2018 Instrument Cluster: [.nan, 1.5, .nan]
NISSAN LEAF 2018: [.nan, 1.5, .nan]
NISSAN ROGUE 2019: [.nan, 1.5, .nan]
# Toyota LTA also has torque
TOYOTA RAV4 2023: [.nan, 3.0, .nan]
TOYOTA RAV4 HYBRID 2023: [.nan, 3.0, .nan]
# Tesla has high torque
TESLA AP1 MODEL S: [.nan, 2.5, .nan]
TESLA AP2 MODEL S: [.nan, 2.5, .nan]
selfdrive/car/torque_data/params.yaml
+1 -1
View File
@@ -30,7 +30,7 @@ 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: [3.078814714619148, 3.2961956260770484, 0.12359762054065548]
HYUNDAI KONA ELECTRIC 2019: [3.078814714619148, 2.307336938253934, 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]
selfdrive/car/torque_data/substitute.yaml
+1
View File
@@ -51,6 +51,7 @@ HONDA E 2020: HONDA CIVIC (BOSCH) 2019
HONDA ODYSSEY CHN 2019: HONDA ODYSSEY 2018
HONDA ODYSSEY HYBRID CHN 2022: HONDA ODYSSEY 2018
BUICK LACROSSE 2017: CHEVROLET VOLT PREMIER 2017
BUICK REGAL ESSENCE 2018: CHEVROLET VOLT PREMIER 2017
CADILLAC ESCALADE ESV 2016: CHEVROLET VOLT PREMIER 2017
CADILLAC ATS Premium Performance 2018: CHEVROLET VOLT PREMIER 2017
selfdrive/car/toyota/carcontroller.py
+5
View File
@@ -104,6 +104,11 @@ class CarController:
apply_steer_req = 0
self.steer_rate_counter = 0
# Never actuate with LKA on cars that only support LTA
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
apply_steer = 0
apply_steer_req = 0
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not CC.enabled and CS.pcm_acc_status:
selfdrive/car/toyota/interface.py
+21 -15
View File
@@ -2,7 +2,8 @@
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.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from common.params import Params
@@ -30,16 +31,22 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)]
ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate]
if candidate in (CAR.RAV4, CAR.PRIUS_V, CAR.COROLLA, CAR.LEXUS_ESH, CAR.LEXUS_CTH):
# BRAKE_MODULE is on a different address for these cars
if DBC[candidate]["pt"] == "toyota_new_mc_pt_generated":
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE
if candidate in ANGLE_CONTROL_CAR:
ret.dashcamOnly = True
ret.steerControlType = car.CarParams.SteerControlType.angle
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
ret.steerLimitTimer = 0.4
ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
stop_and_go = False
steering_angle_deadzone_deg = 0.0
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
params = Params()
if candidate == CAR.PRIUS:
@@ -50,15 +57,13 @@ 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 = 0.2
ret.steerActuatorDelay = 0.25
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
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 = 0.2
ret.steerActuatorDelay = 0.25
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
elif candidate == CAR.PRIUS_V:
stop_and_go = True
@@ -118,7 +123,8 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.7983
ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022):
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.RAV4_TSS2_2023, CAR.RAV4H_TSS2_2023):
stop_and_go = True
ret.wheelbase = 2.68986
ret.steerRatio = 14.3
@@ -202,7 +208,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.444
ret.mass = 4305. * CV.LB_TO_KG + STD_CARGO_KG
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection, steering_angle_deadzone_deg)
CarInterfaceBase.dp_lat_tune_collection(candidate, ret.latTuneCollection, steering_angle_deadzone_deg = 0.0)
CarInterfaceBase.configure_dp_tune(ret.lateralTuning, ret.latTuneCollection)
ret.centerToFront = ret.wheelbase * 0.44
@@ -245,15 +251,15 @@ class CarInterface(CarInterfaceBase):
tune.deadzoneBP = [0., 9.]
tune.deadzoneV = [.0, .15]
if candidate in TSS2_CAR or ret.enableGasInterceptor:
tune.kpBP = [0., 5., 20., 30.]
tune.kpV = [1.3, 1.0, 0.7, 0.1]
tune.kiBP = [0., 3.1, 13.9, 19.4, 30., 33., 40.]
tune.kiV = [.032, .073, .16, .176, .01, .005, .0005]
tune.kpBP = [0., 5., 20.]
tune.kpV = [1.3, 1.0, 0.7]
tune.kiBP = [0., 3., 4., 5., 12., 20., 23., 40.]
tune.kiV = [.08, .16, .26, .215, .20, .166, .1, .006]
if candidate in TSS2_CAR:
#ret.vEgoStopping = 0.3 # car is near 0.1 to 0.2 when car starts requesting stopping accel
ret.vEgoStarting = 0.1 # needs to be > or == vEgoStopping
#ret.stopAccel = -0.1 # Toyota requests -0.4 when stopped
ret.stoppingDecelRate = 0.01 # reach stopping target smoothly - seems to take 0.5 seconds to go from 0 to -0.4
ret.stoppingDecelRate = 0.04 # 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 ###
selfdrive/car/toyota/toyotacan.py
+1 -1
View File
@@ -17,7 +17,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
"SETME_X1": 1,
"SETME_X3": 3,
"PERCENTAGE": 100,
"SETME_X64": 0x64,
"SETME_X64": 0,
"ANGLE": 0,
"STEER_ANGLE_CMD": steer,
"STEER_REQUEST": steer_req,
selfdrive/car/toyota/values.py
+60 -6
View File
@@ -70,8 +70,10 @@ class CAR:
RAV4H = "TOYOTA RAV4 HYBRID 2017"
RAV4_TSS2 = "TOYOTA RAV4 2019"
RAV4_TSS2_2022 = "TOYOTA RAV4 2022"
RAV4_TSS2_2023 = "TOYOTA RAV4 2023"
RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
RAV4H_TSS2_2022 = "TOYOTA RAV4 HYBRID 2022"
RAV4H_TSS2_2023 = "TOYOTA RAV4 HYBRID 2023"
MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5
SIENNA = "TOYOTA SIENNA 2018"
@@ -160,14 +162,16 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
],
CAR.RAV4_TSS2: ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
CAR.RAV4_TSS2_2022: ToyotaCarInfo("Toyota RAV4 2022"),
CAR.RAV4_TSS2_2023: ToyotaCarInfo("Toyota RAV4 2023"),
CAR.RAV4H_TSS2: ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
CAR.RAV4H_TSS2_2022: ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"),
CAR.RAV4H_TSS2_2023: ToyotaCarInfo("Toyota RAV4 Hybrid 2023"),
CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"),
CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED),
# Lexus
CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "Lexus Safety System+"),
CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18", "Lexus Safety System+"),
CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18"),
CAR.LEXUS_ES_TSS2: ToyotaCarInfo("Lexus ES 2019-22"),
CAR.LEXUS_ESH_TSS2: ToyotaCarInfo("Lexus ES Hybrid 2019-23", video_link="https://youtu.be/BZ29osRVJeg?t=12"),
CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"),
@@ -175,7 +179,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
CAR.LEXUS_NXH: ToyotaCarInfo("Lexus NX Hybrid 2018-19"),
CAR.LEXUS_NX_TSS2: ToyotaCarInfo("Lexus NX 2020-21"),
CAR.LEXUS_NXH_TSS2: ToyotaCarInfo("Lexus NX Hybrid 2020-21"),
CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2017-20"),
CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2018-20"),
CAR.LEXUS_RX: [
ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+"),
ToyotaCarInfo("Lexus RX 2017-19"),
@@ -530,14 +534,18 @@ FW_VERSIONS = {
CAR.CAMRY_TSS2: {
(Ecu.eps, 0x7a1, None): [
b'8965B33630\x00\x00\x00\x00\x00\x00',
b'8965B33640\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'\x01F152606370\x00\x00\x00\x00\x00\x00',
b'\x01F152606390\x00\x00\x00\x00\x00\x00',
b'\x01F152606400\x00\x00\x00\x00\x00\x00',
b'\x01F152606431\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x018966306Q5000\x00\x00\x00\x00',
b'\x018966306Q9000\x00\x00\x00\x00',
b'\x018966306R3000\x00\x00\x00\x00',
b'\x018966306T3100\x00\x00\x00\x00',
b'\x018966306T3200\x00\x00\x00\x00',
b'\x018966306T4000\x00\x00\x00\x00',
@@ -545,12 +553,15 @@ FW_VERSIONS = {
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F6201200\x00\x00\x00\x00',
b'\x018821F6201300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0602100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F0602200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305200\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
b'\x028646F3305500\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
],
},
CAR.CAMRYH_TSS2: {
@@ -1457,6 +1468,23 @@ FW_VERSIONS = {
b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
],
},
CAR.RAV4_TSS2_2023: {
(Ecu.abs, 0x7b0, None): [
b'\x01F15260R450\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634AJ2000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F0R03100\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00',
],
},
CAR.RAV4H_TSS2: {
(Ecu.engine, 0x700, None): [
b'\x01896634A15000\x00\x00\x00\x00',
@@ -1544,6 +1572,23 @@ FW_VERSIONS = {
b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00',
],
},
CAR.RAV4H_TSS2_2023: {
(Ecu.abs, 0x7b0, None): [
b'\x01F15264283200\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896634AE1001\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F0R03100\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0R05100\x00\x00\x00\x008646G0R02100\x00\x00\x00\x00',
],
},
CAR.SIENNA: {
(Ecu.engine, 0x700, None): [
b'\x01896630832100\x00\x00\x00\x00',
@@ -1640,6 +1685,7 @@ FW_VERSIONS = {
b'\x028966333S8000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
b'\x028966333T0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
b'\x028966333V4000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
b'\x028966333W1000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
b'\x02896633T09000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00',
b'\x01896633T38000\x00\x00\x00\x00',
b'\x01896633T58000\x00\x00\x00\x00',
@@ -1731,6 +1777,7 @@ FW_VERSIONS = {
b'\x018966378B2100\x00\x00\x00\x00',
b'\x018966378B3000\x00\x00\x00\x00',
b'\x018966378B4100\x00\x00\x00\x00',
b'\x018966378G2000\x00\x00\x00\x00',
b'\x018966378G3000\x00\x00\x00\x00',
b'\x018966378B2000\x00\x00\x00\x00',
],
@@ -1753,6 +1800,7 @@ FW_VERSIONS = {
CAR.LEXUS_NXH_TSS2: {
(Ecu.engine, 0x7e0, None): [
b'\x0237887000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
b'\x02378A0000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'F152678210\x00\x00\x00\x00\x00\x00',
@@ -1762,6 +1810,7 @@ FW_VERSIONS = {
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301400\x00\x00\x00\x00',
b'\x018821F3301300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
@@ -2098,6 +2147,7 @@ DBC = {
CAR.AVALONH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.RAV4_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
@@ -2108,6 +2158,7 @@ DBC = {
CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4H_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.RAV4H_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.LEXUS_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
@@ -2122,8 +2173,8 @@ DBC = {
EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100})
# Toyota/Lexus Safety Sense 2.0 and 2.5
TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022,
CAR.RAV4H_TSS2_2023, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.LEXUS_NXH_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2, CAR.ALPHARDH_TSS2, CAR.CHR_TSS2, CAR.CHRH_TSS2}
NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
@@ -2132,10 +2183,13 @@ NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC}
# these cars have a radar which sends ACC messages instead of the camera
RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.CHR_TSS2, CAR.CHRH_TSS2}
RADAR_ACC_CAR = {CAR.RAV4H_TSS2_2022, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2, CAR.CHRH_TSS2}
# these cars use the Lane Tracing Assist (LTA) message for lateral control
ANGLE_CONTROL_CAR = {CAR.RAV4H_TSS2_2023, CAR.RAV4_TSS2_2023}
EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.CHRH_TSS2, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS,
CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH,
CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022, CAR.RAV4H_TSS2_2023, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH,
CAR.LEXUS_RXH_TSS2, CAR.LEXUS_NXH_TSS2, CAR.PRIUS_TSS2, CAR.ALPHARDH_TSS2}
# no resume button press required
selfdrive/car/volkswagen/carstate.py
+5 -5
View File
@@ -230,13 +230,13 @@ class CarState(CarStateBase):
ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"])
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)
ret.accFaulted = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_StaACC"] in (6, 7)
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.
ret.cruiseState.speed = ext_cp.vl["ACC_GRA_Anziege"]["ACA_V_Wunsch"] * CV.KPH_TO_MS
ret.cruiseState.speed = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_V_Wunsch"] * CV.KPH_TO_MS
if ret.cruiseState.speed > 70: # 255 kph in m/s == no current setpoint
ret.cruiseState.speed = 0
@@ -520,12 +520,12 @@ class PqExtraSignals:
# Additional signal and message lists for optional or bus-portable controllers
fwd_radar_signals = [
("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
("ACA_StaACC", "ACC_GRA_Anzeige"), # ACC drivetrain coordinator status
("ACA_V_Wunsch", "ACC_GRA_Anzeige"), # ACC set speed
]
fwd_radar_checks = [
("ACC_System", 50), # From J428 ACC radar control module
("ACC_GRA_Anziege", 25), # From J428 ACC radar control module
("ACC_GRA_Anzeige", 25), # From J428 ACC radar control module
]
bsm_radar_signals = [
("SWA_Infostufe_SWA_li", "SWA_1"), # Blind spot object info, left
selfdrive/car/volkswagen/pqcan.py
+1 -1
View File
@@ -88,4 +88,4 @@ def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance
# kmh_mph handling probably needed to resolve rounding errors in displayed setpoint
}
return packer.make_can_msg("ACC_GRA_Anziege", bus, values)
return packer.make_can_msg("ACC_GRA_Anzeige", bus, values)
selfdrive/car/volkswagen/values.py
+14 -2
View File
@@ -36,7 +36,7 @@ class CarControllerParams:
if CP.carFingerprint in PQ_CARS:
self.LDW_STEP = 5 # LDW_1 message frequency 20Hz
self.ACC_HUD_STEP = 4 # ACC_GRA_Anziege frequency 25Hz
self.ACC_HUD_STEP = 4 # ACC_GRA_Anzeige frequency 25Hz
self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm
self.STEER_DELTA_UP = 6 # Max HCA reached in 1.00s (STEER_MAX / (50Hz * 1.00))
self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))
@@ -300,6 +300,7 @@ FW_VERSIONS = {
b'\xf1\x873G0906259N \xf1\x890004',
b'\xf1\x873G0906259P \xf1\x890001',
b'\xf1\x875NA907115H \xf1\x890002',
b'\xf1\x873G0906259G \xf1\x890004',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x8709G927158L \xf1\x893611',
@@ -311,17 +312,19 @@ FW_VERSIONS = {
b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1616001613121157161111572900',
b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1616001613121177161113772900',
b'\xf1\x873Q0959655DL\xf1\x890732\xf1\x82\0161812141812171105141123052J00',
b'\xf1\x873Q0959655CK\xf1\x890711\xf1\x82\x0e1712141712141105121122052900',
],
(Ecu.eps, 0x712, None): [
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\x875Q0910143C \xf1\x892211\xf1\x82\x0567B0020800',
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',
b'\xf1\x875Q0907572R \xf1\x890771',
],
},
CAR.ATLAS_MK1: {
@@ -401,6 +404,7 @@ FW_VERSIONS = {
b'\xf1\x8704L906021DT\xf1\x895520',
b'\xf1\x8704L906021DT\xf1\x898127',
b'\xf1\x8704L906021N \xf1\x895518',
b'\xf1\x8704L906026BN\xf1\x891197',
b'\xf1\x8704L906026BP\xf1\x897608',
b'\xf1\x8704L906026NF\xf1\x899528',
b'\xf1\x8704L906056CL\xf1\x893823',
@@ -521,6 +525,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\x0511A00403A0',
b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\x0516A00604A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00404A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00504A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A00604A1',
b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516A07A02A1',
b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521A00507A1',
@@ -1080,6 +1085,8 @@ FW_VERSIONS = {
b'\xf1\x8704L906026DE\xf1\x895418',
b'\xf1\x8704L906026EJ\xf1\x893661',
b'\xf1\x8704L906026HT\xf1\x893617',
b'\xf1\x8783A907115E \xf1\x890001',
b'\xf1\x8705E906018DJ\xf1\x890915',
b'\xf1\x875NA907115E \xf1\x890003',
b'\xf1\x875NA907115E \xf1\x890005',
],
@@ -1089,6 +1096,8 @@ FW_VERSIONS = {
b'\xf1\x870DL300012M \xf1\x892107',
b'\xf1\x870DL300012N \xf1\x892110',
b'\xf1\x870DL300013G \xf1\x892119',
b'\xf1\x870GC300014N \xf1\x892801',
b'\xf1\x870GC300046Q \xf1\x892802',
],
(Ecu.srs, 0x715, None): [
b'\xf1\x873Q0959655AP\xf1\x890306\xf1\x82\r11110011110011421111314211',
@@ -1096,11 +1105,13 @@ FW_VERSIONS = {
b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e1213001211001244212111442100',
b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e1213001211001205212112052100',
b'\xf1\x873Q0959655CQ\xf1\x890720\xf1\x82\x0e1213111211001205212112052111',
b'\xf1\x873Q0959655DJ\xf1\x890731\xf1\x82\x0e1513001511001205232113052J00',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6050405',
b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6060405',
b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6070405',
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G500',
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G600',
],
(Ecu.fwdRadar, 0x757, None): [
@@ -1108,6 +1119,7 @@ FW_VERSIONS = {
b'\xf1\x872Q0907572R \xf1\x890372',
b'\xf1\x872Q0907572T \xf1\x890383',
b'\xf1\x872Q0907572AA\xf1\x890396',
b'\xf1\x872Q0907572AB\xf1\x890397',
],
},
CAR.SKODA_OCTAVIA_MK3: {
selfdrive/controls/controlsd.py
+13
View File
@@ -136,6 +136,9 @@ class Controls:
self.dp_lateral_alt_v_cruise_kph = 0
self.dp_lateral_alt_v_cruise_kph_prev = 0
self.dp_lateral_alt_active = False
self.local_trip_min_total = float(self.params.get("local_trip_min_total", encoding='utf8'))
self.local_trip_meter_total = float(self.params.get("local_trip_meter_total", encoding='utf8'))
self.local_trip_count_added = False
# read params
self.is_metric = self.params.get_bool("IsMetric")
@@ -544,6 +547,16 @@ class Controls:
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
# dp - local trip log
self.local_trip_meter_total += CS.vEgo * DT_CTRL
if not self.local_trip_count_added:
if self.distance_traveled > 0:
put_nonblocking("local_trip_count_total", str(float(self.params.get("local_trip_count_total").decode('utf-8')) + 1))
self.local_trip_count_added = True
# every 30 secs
if self.local_trip_count_added and self.sm.frame % int(30. / DT_CTRL) == 0:
put_nonblocking("local_trip_meter_total", str(round(self.local_trip_meter_total, 2)))
put_nonblocking("local_trip_min_total", str(self.local_trip_min_total + 0.5))
return CS
selfdrive/controls/lib/events.py
+2 -2
View File
@@ -913,9 +913,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
ET.PERMANENT: Alert(
_("Reverse\nGear"),
"",
AlertStatus.normal, AlertSize.none,
AlertStatus.normal, AlertSize.full,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
# ET.USER_DISABLE: ImmediateDisableAlert(_("Reverse Gear")),
ET.USER_DISABLE: ImmediateDisableAlert(_("Reverse Gear")),
ET.NO_ENTRY: NoEntryAlert(_("Reverse Gear")),
},
selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json
+544
View File
@@ -0,0 +1,544 @@
{
"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
]
}
}
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
+278
View File
@@ -0,0 +1,278 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados_c/external_function_interface.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/print.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_sim_solver_lat.h"
// ** solver data **
sim_solver_capsule * lat_acados_sim_solver_create_capsule()
{
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
return capsule;
}
int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
{
free(capsule);
return 0;
}
int lat_acados_sim_create(sim_solver_capsule * capsule)
{
// initialize
const int nx = LAT_NX;
const int nu = LAT_NU;
const int nz = LAT_NZ;
const int np = LAT_NP;
bool tmp_bool;
double Tsim = 0.009765625;
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
// sim plan & config
sim_solver_plan_t plan;
plan.sim_solver = ERK;
// create correct config based on plan
sim_config * lat_sim_config = sim_config_create(plan);
capsule->acados_sim_config = lat_sim_config;
// sim dims
void *lat_sim_dims = sim_dims_create(lat_sim_config);
capsule->acados_sim_dims = lat_sim_dims;
sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
// sim opts
sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_opts = lat_sim_opts;
int tmp_int = 3;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
tmp_int = 4;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
tmp_int = 1;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
tmp_bool = 0;
sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
// sim in / out
sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_in = lat_sim_in;
sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_out = lat_sim_out;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "T", &Tsim);
// model functions
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
// sim solver
sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
lat_sim_dims, lat_sim_opts);
capsule->acados_sim_solver = lat_sim_solver;
/* initialize parameter values */
double* p = calloc(np, sizeof(double));
lat_acados_sim_update_params(capsule, p, np);
free(p);
/* initialize input */
// x
double x0[4];
for (int ii = 0; ii < 4; ii++)
x0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "x", x0);
// u
double u0[1];
for (int ii = 0; ii < 1; ii++)
u0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "u", u0);
// S_forw
double S_forw[20];
for (int ii = 0; ii < 20; ii++)
S_forw[ii] = 0.0;
for (int ii = 0; ii < 4; ii++)
S_forw[ii + ii * 4 ] = 1.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "S_forw", S_forw);
int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
return status;
}
int lat_acados_sim_solve(sim_solver_capsule *capsule)
{
// integrate dynamics using acados sim_solver
int status = sim_solve(capsule->acados_sim_solver,
capsule->acados_sim_in, capsule->acados_sim_out);
if (status != 0)
printf("error in lat_acados_sim_solve()! Exiting.\n");
return status;
}
int lat_acados_sim_free(sim_solver_capsule *capsule)
{
// free memory
sim_solver_destroy(capsule->acados_sim_solver);
sim_in_destroy(capsule->acados_sim_in);
sim_out_destroy(capsule->acados_sim_out);
sim_opts_destroy(capsule->acados_sim_opts);
sim_dims_destroy(capsule->acados_sim_dims);
sim_config_destroy(capsule->acados_sim_config);
// free external function
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
return 0;
}
int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
{
int status = 0;
int casadi_np = LAT_NP;
if (casadi_np != np) {
printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
return status;
}
/* getters pointers to C objects*/
sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
{
return capsule->acados_sim_config;
};
sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
{
return capsule->acados_sim_in;
};
sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
{
return capsule->acados_sim_out;
};
void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
{
return capsule->acados_sim_dims;
};
sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
{
return capsule->acados_sim_opts;
};
sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
{
return capsule->acados_sim_solver;
};
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
+103
View File
@@ -0,0 +1,103 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_lat_H_
#define ACADOS_SIM_lat_H_
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#define LAT_NX 4
#define LAT_NZ 0
#define LAT_NU 1
#define LAT_NP 2
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct sim_solver_capsule
{
// acados objects
sim_in *acados_sim_in;
sim_out *acados_sim_out;
sim_solver *acados_sim_solver;
sim_opts *acados_sim_opts;
sim_config *acados_sim_config;
void *acados_sim_dims;
/* external functions */
// ERK
external_function_param_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
// IRK
external_function_param_casadi * sim_impl_dae_fun;
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
external_function_param_casadi * sim_impl_dae_hess;
// GNSF
external_function_param_casadi * sim_gnsf_phi_fun;
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi * sim_gnsf_get_matrices_fun;
} sim_solver_capsule;
ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_SIM_lat_H_
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_lat.c
-16
View File
@@ -405,22 +405,6 @@ void lat_acados_create_5_set_nlp_in(lat_solver_capsule* capsule, const int N, do
time_steps[13] = 0.263671875;
time_steps[14] = 0.283203125;
time_steps[15] = 0.302734375;
time_steps[16] = 0.322265625;
time_steps[17] = 0.341796875;
time_steps[18] = 0.361328125;
time_steps[19] = 0.380859375;
time_steps[20] = 0.400390625;
time_steps[21] = 0.419921875;
time_steps[22] = 0.439453125;
time_steps[23] = 0.458984375;
time_steps[24] = 0.478515625;
time_steps[25] = 0.498046875;
time_steps[26] = 0.517578125;
time_steps[27] = 0.537109375;
time_steps[28] = 0.556640625;
time_steps[29] = 0.576171875;
time_steps[30] = 0.595703125;
time_steps[31] = 0.615234375;
lat_acados_update_time_steps(capsule, N, time_steps);
free(time_steps);
}
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_lat.h
+1 -1
View File
@@ -63,7 +63,7 @@
#define LAT_NY0 5
#define LAT_NY 5
#define LAT_NYN 3
#define LAT_N 32
#define LAT_N 16
#define LAT_NH 0
#define LAT_NPHI 0
#define LAT_NHN 0
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
+264
View File
@@ -0,0 +1,264 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#define S_FUNCTION_NAME acados_solver_sfunction_lat
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_solver_lat.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.009765625
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);// specify the number of input ports
if ( !ssSetNumInputPorts(S, 8) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 6) )
return;
// specify dimension information for the input ports
// lbx_0
ssSetInputPortVectorDimension(S, 0, 4);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 4);
// parameters
ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 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
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/libacados_ocp_solver_lat.so
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
+128
View File
@@ -0,0 +1,128 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/sim_interface.h"
#include "acados_sim_solver_lat.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
#define NP LAT_NP
int main()
{
int status = 0;
sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
status = lat_acados_sim_create(capsule);
if (status)
{
printf("acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
// initial condition
double x_current[NX];
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
x_current[3] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[3] = 0;
// initial value for control input
double u0[NU];
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
lat_acados_sim_update_params(capsule, p, NP);
int n_sim_steps = 3;
// solve ocp in loop
for (int ii = 0; ii < n_sim_steps; ii++)
{
sim_in_set(acados_sim_config, acados_sim_dims,
acados_sim_in, "x", x_current);
status = lat_acados_sim_solve(capsule);
if (status != ACADOS_SUCCESS)
{
printf("acados_solve() failed with status %d.\n", status);
}
sim_out_get(acados_sim_config, acados_sim_dims,
acados_sim_out, "x", x_current);
printf("\nx_current, %d\n", ii);
for (int jj = 0; jj < NX; jj++)
{
printf("%e\n", x_current[jj]);
}
}
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
// free solver
status = lat_acados_sim_free(capsule);
if (status) {
printf("lat_acados_sim_free() returned status %d. \n", status);
}
lat_acados_sim_solver_free_capsule(capsule);
return status;
}
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
+125
View File
@@ -0,0 +1,125 @@
%
% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
%
% This file is part of acados.
%
% The 2-Clause BSD License
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.;
%
SOURCES = { ...
'lat_model/lat_expl_ode_fun.c', ...
'lat_model/lat_expl_vde_forw.c',...
'lat_cost/lat_cost_y_0_fun.c',...
'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_0_hess.c',...
'lat_cost/lat_cost_y_fun.c',...
'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_hess.c',...
'lat_cost/lat_cost_y_e_fun.c',...
'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_e_hess.c',...
'acados_solver_sfunction_lat.c', ...
'acados_solver_lat.c'
};
INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
['-I', fullfile(INC_PATH, 'acados')], ...
['-I', fullfile(INC_PATH)]};
CFLAGS = 'CFLAGS=$CFLAGS';
LDFLAGS = 'LDFLAGS=$LDFLAGS';
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
% acados linking libraries and flags
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_lat' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
' size [34]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [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)
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
+4 -4
View File
@@ -17,12 +17,12 @@ EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
X_DIM = 4
P_DIM = 2
N = 16
COST_E_DIM = 3
COST_DIM = COST_E_DIM + 2
SPEED_OFFSET = 10.0
MODEL_NAME = 'lat'
ACADOS_SOLVER_TYPE = 'SQP_RTI'
N = 32
def gen_lat_model():
model = AcadosModel()
@@ -168,14 +168,14 @@ class LateralMpc():
self.solver.constraints_set(0, "lbx", x0_cp)
self.solver.constraints_set(0, "ubx", x0_cp)
self.yref[:,0] = y_pts
v_ego = p_cp[0, 0]
v_ego = p_cp[0]
# rotation_radius = p_cp[1]
self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET)
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.set(i, "p", p_cp[i])
self.solver.set(N, "p", p_cp[N])
self.solver.set(i, "p", p_cp)
self.solver.set(N, "p", p_cp)
self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
t = sec_since_boot()
selfdrive/controls/lib/lateral_planner.py
+10 -14
View File
@@ -9,7 +9,7 @@ from selfdrive.controls.lib.desire_helper import DesireHelper
import cereal.messaging as messaging
from cereal import log
from selfdrive.controls.lib.lane_planner import LanePlanner
from system.hardware import TICI
from selfdrive.hardware import TICI
from common.params import Params
TRAJECTORY_SIZE = 33
@@ -19,12 +19,12 @@ CAMERA_OFFSET = 0.04
PATH_COST = 1.0
LATERAL_MOTION_COST = 0.11
LATERAL_ACCEL_COST = 0.0
LATERAL_JERK_COST = 0.04
LATERAL_JERK_COST = 0.05
# Extreme steering rate is unpleasant, even
# when it does not cause bad jerk.
# TODO this cost should be lowered when low
# speed lateral control is stable on all cars
STEERING_RATE_COST = 700.0
STEERING_RATE_COST = 800.0
class LateralPlanner:
@@ -46,7 +46,6 @@ class LateralPlanner:
self.solution_invalid_cnt = 0
self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE)
@@ -62,6 +61,7 @@ class LateralPlanner:
def update(self, sm):
# clip speed , lateral planning is not possible at 0 speed
self.v_ego = max(MIN_SPEED, sm['carState'].vEgo)
measured_curvature = sm['controlsState'].curvature
if sm.updated['dragonConf']:
self.dp_lanelines_enable = sm['dragonConf'].dpLateralLanelines
@@ -79,16 +79,12 @@ class LateralPlanner:
self.t_idxs = np.array(md.position.t)
self.plan_yaw = np.array(md.orientation.z)
self.plan_yaw_rate = np.array(md.orientationRate.z)
self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
car_speed = np.linalg.norm(self.velocity_xyz, axis=1)
self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
self.v_ego = self.v_plan[0]
if self.dp_lanelines_enable:
# dp - when laneline mode enabled, we use old logic (including lane changing)
d_path_xyz = self.lanelines_mode(md, sm['carState'], sm['carControl'].latActive, sm['dragonConf'])
else:
# dp -- tab spacing begin (stock logic) --
# dp -- tab spacing begin (stock logic) --
# Lane change logic
desire_state = md.meta.desireState
if len(desire_state):
@@ -103,16 +99,16 @@ class LateralPlanner:
LATERAL_ACCEL_COST, LATERAL_JERK_COST,
STEERING_RATE_COST)
y_pts = d_path_xyz[:LAT_MPC_N+1, 1]
heading_pts = self.plan_yaw[:LAT_MPC_N+1]
yaw_rate_pts = self.plan_yaw_rate[:LAT_MPC_N+1]
y_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
heading_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
yaw_rate_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate)
self.y_pts = y_pts
assert len(y_pts) == LAT_MPC_N + 1
assert len(heading_pts) == LAT_MPC_N + 1
assert len(yaw_rate_pts) == LAT_MPC_N + 1
lateral_factor = np.clip(self.factor1 - (self.factor2 * self.v_plan**2), 0.0, np.inf)
p = np.column_stack([self.v_plan, lateral_factor])
lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2))
p = np.array([self.v_ego, lateral_factor])
self.lat_mpc.run(self.x0,
p,
y_pts,
selfdrive/controls/lib/longitudinal_planner.py
+8 -8
View File
@@ -40,14 +40,14 @@ DP_ACCEL_NORMAL = 1
DP_ACCEL_SPORT = 2
# accel profile by @arne182 modified by cgw
_DP_CRUISE_MIN_V = [-0.50, -0.50, -0.50, -0.50, -0.50, -0.45, -0.45]
_DP_CRUISE_MIN_V_ECO = [-0.40, -0.40, -0.40, -0.40, -0.40, -0.40, -0.40]
_DP_CRUISE_MIN_V_SPORT = [-0.52, -0.52, -0.52, -0.52, -0.52, -0.47, -0.47]
_DP_CRUISE_MIN_BP = [0., 0.07, 3.1, 10., 20., 30., 55.]
_DP_CRUISE_MIN_V = [-0.6, -0.6, -0.7, -0.8, -0.8, -0.5]
_DP_CRUISE_MIN_V_ECO = [-0.5, -0.5, -0.6, -0.7, -0.7, -0.45]
_DP_CRUISE_MIN_V_SPORT = [-0.7, -0.7, -0.8, -0.9, -0.9, -0.6]
_DP_CRUISE_MIN_BP = [0., 8.3, 14, 20., 30., 55.]
_DP_CRUISE_MAX_V = [3.5, 3.4, 2.1, 1.6, 1.1, 0.91, 0.69, 0.45, 0.34, 0.13]
_DP_CRUISE_MAX_V = [3.5, 3.4, 2.1, 1.6, 1.1, 0.91, 0.69, 0.44, 0.34, 0.13]
_DP_CRUISE_MAX_V_ECO = [3.0, 1.8, 1.3, 1.0, 0.71, 0.59, 0.45, 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_V_SPORT = [3.5, 3.5, 3.4, 3.0, 2.1, 1.7, 1.3, 0.9, 0.7, 0.5]
_DP_CRUISE_MAX_BP = [0., 3, 6., 8., 11., 15., 20., 25., 30., 55.]
# d-e2e, from modeldata.h
@@ -240,8 +240,8 @@ class LongitudinalPlanner:
return desired_tf
if self.dp_following_profile_ctrl:
if self.dp_following_profile == 0:
x_vel = [1.1, 13.89, 25.0, 41.67]
y_dist = [1.35, 1.35, 1.27, 1.32]
x_vel = [1.1, 3.3, 5.5, 13.89, 19.7, 25.0, 41.67]
y_dist = [1.0, 1.2, 1.3, 1.34, 1.34, 1.23, 1.34]
desired_tf = np.interp(v_ego, x_vel, y_dist)
elif self.dp_following_profile == 1:
x_vel = [5.556, 19.7, 41.67]
selfdrive/controls/plannerd.py
+3 -2
View File
@@ -23,6 +23,7 @@ def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
ui_send = messaging.new_message('uiPlan')
ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
uiPlan = ui_send.uiPlan
uiPlan.frameId = sm['modelV2'].frameId
uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,0]).tolist()
uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,1]).tolist()
uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist()
@@ -66,8 +67,8 @@ def plannerd_thread(sm=None, pm=None):
lateral_planner.publish(sm, pm)
longitudinal_planner.update(sm)
longitudinal_planner.publish(sm, pm)
if lat_version == 0:
publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner)
# if lat_version == 0:
# publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner)
def main(sm=None, pm=None):
plannerd_thread(sm, pm)
selfdrive/debug/hyundai_enable_radar_points.py
+3
View File
@@ -32,6 +32,9 @@ SUPPORTED_FW_VERSIONS = {
b"DN8_ SCC FHCUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues(
default_config=b"\x00\x00\x00\x01\x00\x00",
tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
b"DN8_ SCC F-CUP 1.00 1.00 99110-L0000\x19\x08)\x15T ": ConfigValues(
default_config=b"\x00\x00\x00\x01\x00\x00",
tracks_enabled=b"\x00\x00\x00\x01\x00\x01"),
# 2021 SONATA HYBRID
b"DNhe SCC FHCUP 1.00 1.02 99110-L5000 \x01#\x15# ": ConfigValues(
default_config=b"\x00\x00\x00\x01\x00\x00",
selfdrive/dragonpilot/bg.jpg
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 84 KiB

selfdrive/locationd/laikad.py
+154 -155
View File
@@ -1,5 +1,4 @@
#!/usr/bin/env python3
import json
import math
import os
import time
@@ -8,7 +7,7 @@ from collections import defaultdict
from concurrent.futures import Future, ProcessPoolExecutor
from datetime import datetime
from enum import IntEnum
from typing import List, Optional
from typing import List, Optional, Dict, Any
import numpy as np
@@ -17,9 +16,9 @@ from common.params import Params, put_nonblocking
from laika import AstroDog
from laika.constants import SECS_IN_HR, SECS_IN_MIN
from laika.downloader import DownloadFailed
from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_gps_ephem, convert_ublox_glonass_ephem, parse_qcom_ephem
from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem
from laika.gps_time import GPSTime
from laika.helpers import ConstellationId
from laika.helpers import ConstellationId, get_sv_id
from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func
from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
@@ -28,15 +27,53 @@ from selfdrive.locationd.models.gnss_kf import States as GStates
from system.swaglog import cloudlog
MAX_TIME_GAP = 10
EPHEMERIS_CACHE = 'LaikadEphemerisV2'
EPHEMERIS_CACHE = 'LaikadEphemerisV3'
DOWNLOADS_CACHE_FOLDER = "/tmp/comma_download_cache/"
CACHE_VERSION = 0.2
POS_FIX_RESIDUAL_THRESHOLD = 100.0
class LogEphemerisType(IntEnum):
nav = 0
nasaUltraRapid = 1
glonassIacUltraRapid = 2
qcom = 3
class EphemerisSource(IntEnum):
gnssChip = 0
internet = 1
cache = 2
unknown = 3
def get_log_eph_type(ephem):
if ephem.eph_type == EphemerisType.NAV:
source_type = LogEphemerisType.nav
elif ephem.eph_type == EphemerisType.QCOM_POLY:
source_type = LogEphemerisType.qcom
else:
assert ephem.file_epoch is not None
file_src = ephem.file_source
if file_src == 'igu': # example nasa: '2214/igu22144_00.sp3.Z'
source_type = LogEphemerisType.nasaUltraRapid
elif file_src == 'Sta': # example nasa: '22166/ultra/Stark_1D_22061518.sp3'
source_type = LogEphemerisType.glonassIacUltraRapid
else:
raise Exception(f"Didn't expect file source {file_src}")
return source_type
def get_log_eph_source(ephem):
if ephem.file_name == 'qcom' or ephem.file_name == 'ublox':
source = EphemerisSource.gnssChip
elif ephem.file_name == EPHEMERIS_CACHE:
source = EphemerisSource.cache
else:
source = EphemerisSource.internet
return source
class Laikad:
def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_navs=True, auto_update=False,
valid_ephem_types=(EphemerisType.NAV,),
def __init__(self, valid_const=(ConstellationId.GPS, ConstellationId.GLONASS), auto_fetch_navs=True, auto_update=False,
valid_ephem_types=(EphemerisType.NAV, EphemerisType.QCOM_POLY),
save_ephemeris=False, use_qcom=False):
"""
valid_const: GNSS constellation which can be used
@@ -51,10 +88,11 @@ class Laikad:
self.auto_fetch_navs = auto_fetch_navs
self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None
self.orbit_fetch_future: Optional[Future] = None
self.last_fetch_navs_t = None
self.got_first_gnss_msg = False
self.last_cached_t = None
self.last_report_time = GPSTime(0, 0)
self.last_fetch_navs_t = GPSTime(0, 0)
self.last_cached_t = GPSTime(0, 0)
self.save_ephemeris = save_ephemeris
self.load_cache()
@@ -64,36 +102,58 @@ class Laikad:
self.last_fix_t = None
self.gps_week = None
self.use_qcom = use_qcom
self.first_log_time = None
self.ttff = -1
def load_cache(self):
if not self.save_ephemeris:
return
cache = Params().get(EPHEMERIS_CACHE)
if not cache:
cache_bytes = Params().get(EPHEMERIS_CACHE)
if not cache_bytes:
return
nav_dict = {}
try:
cache = json.loads(cache, object_hook=deserialize_hook)
if cache['version'] == CACHE_VERSION:
self.astro_dog.add_navs(cache['navs'])
self.last_fetch_navs_t = cache['last_fetch_navs_t']
else:
cache['navs'] = {}
except json.decoder.JSONDecodeError:
ephem_cache = ephemeris_structs.EphemerisCache.from_bytes(cache_bytes)
glonass_navs = [GLONASSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.glonassEphemerides]
gps_navs = [GPSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.gpsEphemerides]
for e in sum([glonass_navs, gps_navs], []):
if e.prn not in nav_dict:
nav_dict[e.prn] = []
nav_dict[e.prn].append(e)
self.astro_dog.add_navs(nav_dict)
except Exception:
cloudlog.exception("Error parsing cache")
timestamp = self.last_fetch_navs_t.as_datetime() if self.last_fetch_navs_t is not None else 'Nan'
cloudlog.debug(
f"Loaded navs ({sum([len(v) for v in cache['navs']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['navs'].keys())} " +
f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.navs_fetched_times._ranges]}")
f"Loaded navs ({sum([len(nav_dict[prn]) for prn in nav_dict.keys()])}). Unique orbit and nav sats: {list(nav_dict.keys())} ")
def cache_ephemeris(self, t: GPSTime):
if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN):
put_nonblocking(EPHEMERIS_CACHE, json.dumps(
{'version': CACHE_VERSION, 'last_fetch_navs_t': self.last_fetch_navs_t, 'navs': self.astro_dog.navs},
cls=CacheSerializer))
def cache_ephemeris(self):
if self.save_ephemeris and (self.last_report_time - self.last_cached_t > SECS_IN_MIN):
nav_list: List = sum([v for k,v in self.astro_dog.navs.items()], [])
ephem_cache = ephemeris_structs.EphemerisCache(**{'glonassEphemerides': [e.data for e in nav_list if e.prn[0]=='R'],
'gpsEphemerides': [e.data for e in nav_list if e.prn[0]=='G']})
put_nonblocking(EPHEMERIS_CACHE, ephem_cache.to_bytes())
cloudlog.debug("Cache saved")
self.last_cached_t = t
self.last_cached_t = self.last_report_time
def create_ephem_statuses(self):
ephemeris_statuses = []
prns_to_check = list(self.astro_dog.get_all_ephem_prns())
prns_to_check.sort()
for prn in prns_to_check:
eph = self.astro_dog.get_eph(prn, self.last_report_time)
if eph is not None:
status = log.GnssMeasurements.EphemerisStatus.new_message()
status.constellationId = ConstellationId.from_rinex_char(prn[0]).value
status.svId = get_sv_id(prn)
status.type = get_log_eph_type(eph).value
status.source = get_log_eph_source(eph).value
ephemeris_statuses.append(status)
return ephemeris_statuses
def get_lsq_fix(self, t, measurements):
if self.last_fix_t is None or abs(self.last_fix_t - t) > 0:
@@ -121,8 +181,15 @@ class Laikad:
def is_good_report(self, gnss_msg):
if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom:
constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
# TODO support GLONASS
return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
# TODO: Understand and use remaining unknown constellations
try:
good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
except NotImplementedError:
good_constellation = False
# gpsWeek 65535 is received rarely from quectel, this cannot be
# passed to GnssMeasurements's gpsWeek (Int16)
good_week = not getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max
return good_constellation and good_week
elif gnss_msg.which() == 'measurementReport' and not self.use_qcom:
return True
else:
@@ -139,6 +206,7 @@ class Laikad:
week = report.gpsWeek
tow = report.rcvTow
new_meas = read_raw_ublox(report)
self.last_report_time = GPSTime(week, tow)
return week, tow, new_meas
def is_ephemeris(self, gnss_msg):
@@ -153,28 +221,34 @@ class Laikad:
if self.gps_week is None:
return
ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week)
self.astro_dog.add_qcom_polys({ephem.prn: [ephem]})
else:
if gnss_msg.which() == 'ephemeris':
ephem = convert_ublox_gps_ephem(gnss_msg.ephemeris)
data_struct = ephemeris_structs.Ephemeris.new_message(**gnss_msg.ephemeris.to_dict())
ephem = GPSEphemeris(data_struct, file_name='ublox')
elif gnss_msg.which() == 'glonassEphemeris':
ephem = convert_ublox_glonass_ephem(gnss_msg.glonassEphemeris)
data_struct = ephemeris_structs.GlonassEphemeris.new_message(**gnss_msg.glonassEphemeris.to_dict())
ephem = GLONASSEphemeris(data_struct, file_name='ublox')
else:
cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}")
return
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris()
def process_report(self, new_meas, t):
# Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
processed_measurements = process_measurements(new_meas, self.astro_dog)
if self.last_fix_pos is not None:
corrected_measurements = correct_measurements(processed_measurements, self.last_fix_pos, self.astro_dog)
instant_fix = self.get_lsq_fix(t, corrected_measurements)
#instant_fix = self.get_lsq_fix(t, processed_measurements)
est_pos = self.last_fix_pos
else:
corrected_measurements = []
instant_fix = self.get_lsq_fix(t, processed_measurements)
est_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist()
corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog)
return corrected_measurements
def calc_fix(self, t, measurements):
instant_fix = self.get_lsq_fix(t, measurements)
if instant_fix is None:
return None
else:
@@ -182,60 +256,53 @@ class Laikad:
self.last_fix_t = t
self.last_fix_pos = position_estimate
self.lat_fix_pos_std = position_std
if (t*1e9) % 10 == 0:
cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}")
return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements
return position_estimate, position_std, velocity_estimate, velocity_std
def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
out_msg = messaging.new_message("gnssMeasurements")
t = gnss_mono_time * 1e-9
msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time}
if self.first_log_time is None:
self.first_log_time = 1e-9 * gnss_mono_time
if self.is_ephemeris(gnss_msg):
self.read_ephemeris(gnss_msg)
return None
elif self.is_good_report(gnss_msg):
week, tow, new_meas = self.read_report(gnss_msg)
self.gps_week = week
if len(new_meas) == 0:
return None
t = gnss_mono_time * 1e-9
if week > 0:
self.got_first_gnss_msg = True
latest_msg_t = GPSTime(week, tow)
if self.auto_fetch_navs:
self.fetch_navs(latest_msg_t, block)
output = self.process_report(new_meas, t)
if output is None:
return None
position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, _ = output
corrected_measurements = self.process_report(new_meas, t)
msg_dict['correctedMeasurements'] = [create_measurement_msg(m) for m in corrected_measurements]
self.update_localizer(position_estimate, t, corrected_measurements)
meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
msg = messaging.new_message("gnssMeasurements")
fix = self.calc_fix(t, corrected_measurements)
measurement_msg = log.LiveLocationKalman.Measurement.new_message
if fix is not None:
position_estimate, position_std, velocity_estimate, velocity_std = fix
if self.ttff <= 0:
self.ttff = max(1e-3, t - self.first_log_time)
msg_dict["positionECEF"] = measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t))
msg_dict["velocityECEF"] = measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t))
self.update_localizer(self.last_fix_pos, t, corrected_measurements)
P_diag = self.gnss_kf.P.diagonal()
kf_valid = all(self.kf_valid(t))
msg.gnssMeasurements = {
"gpsWeek": week,
"gpsTimeOfWeek": tow,
"kalmanPositionECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(),
msg_dict["kalmanPositionECEF"] = measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(),
std=np.sqrt(P_diag[GStates.ECEF_POS]).tolist(),
valid=kf_valid),
"kalmanVelocityECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist(),
valid=kf_valid)
msg_dict["kalmanVelocityECEF"] = measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist(),
std=np.sqrt(P_diag[GStates.ECEF_VELOCITY]).tolist(),
valid=kf_valid),
"positionECEF": measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t)),
"velocityECEF": measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t)),
"measTime": gnss_mono_time,
"correctedMeasurements": meas_msgs
}
return msg
#elif gnss_msg.which() == 'ionoData':
# TODO: add this, Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
valid=kf_valid)
msg_dict['gpsWeek'] = self.last_report_time.week
msg_dict['gpsTimeOfWeek'] = self.last_report_time.tow
msg_dict['timeToFirstFix'] = self.ttff
msg_dict['ephemerisStatuses'] = self.create_ephem_statuses()
out_msg.gnssMeasurements = msg_dict
return out_msg
def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]):
# Check time and outputs are valid
@@ -247,7 +314,7 @@ class Laikad:
cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
elif not valid[2]:
cloudlog.error("Gnss kalman filter state is nan")
if len(est_pos) > 0:
if est_pos is not None and len(est_pos) > 0:
cloudlog.info(f"Reset kalman filter with {est_pos}")
self.init_gnss_localizer(est_pos)
else:
@@ -272,7 +339,7 @@ class Laikad:
def fetch_navs(self, t: GPSTime, block):
# Download new navs if 1 hour of navs data left
if t + SECS_IN_HR not in self.astro_dog.navs_fetched_times and (self.last_fetch_navs_t is None or abs(t - self.last_fetch_navs_t) > SECS_IN_MIN):
if t + SECS_IN_HR not in self.astro_dog.navs_fetched_times and (abs(t - self.last_fetch_navs_t) > SECS_IN_MIN):
astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types, self.astro_dog.cache_dir
ret = None
@@ -290,7 +357,7 @@ class Laikad:
self.last_fetch_navs_t = ret[2]
else:
self.astro_dog.navs, self.astro_dog.navs_fetched_times, self.last_fetch_navs_t = ret
self.cache_ephemeris(t=t)
self.cache_ephemeris()
def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir):
@@ -320,31 +387,8 @@ def create_measurement_msg(meas: GNSSMeasurement):
c.satPos = meas.sat_pos_final.tolist()
c.satVel = meas.sat_vel.tolist()
c.satVel = meas.sat_vel.tolist()
ephem = meas.sat_ephemeris
assert ephem is not None
week, time_of_week = -1, -1
if ephem.eph_type == EphemerisType.NAV:
source_type = EphemerisSourceType.nav
elif ephem.eph_type == EphemerisType.QCOM_POLY:
source_type = EphemerisSourceType.qcompoly
else:
assert ephem.file_epoch is not None
week = ephem.file_epoch.week
time_of_week = ephem.file_epoch.tow
file_src = ephem.file_source
if file_src == 'igu': # example nasa: '2214/igu22144_00.sp3.Z'
source_type = EphemerisSourceType.nasaUltraRapid
elif file_src == 'Sta': # example nasa: '22166/ultra/Stark_1D_22061518.sp3'
source_type = EphemerisSourceType.glonassIacUltraRapid
else:
raise Exception(f"Didn't expect file source {file_src}")
c.ephemerisSource.type = source_type.value
c.ephemerisSource.gpsWeek = week
c.ephemerisSource.gpsTimeOfWeek = int(time_of_week)
return c
def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMeasurement]):
ekf_data = defaultdict(list)
for m in measurements:
@@ -360,69 +404,29 @@ def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMe
gnss_kf.predict_and_observe(t, kind, data)
class CacheSerializer(json.JSONEncoder):
def default(self, o):
if isinstance(o, Ephemeris):
return o.to_json()
if isinstance(o, GPSTime):
return o.__dict__
if isinstance(o, np.ndarray):
return o.tolist()
return json.JSONEncoder.default(self, o)
def deserialize_hook(dct):
if 'ephemeris' in dct:
return Ephemeris.from_json(dct)
if 'week' in dct:
return GPSTime(dct['week'], dct['tow'])
return dct
class EphemerisSourceType(IntEnum):
nav = 0
nasaUltraRapid = 1
glonassIacUltraRapid = 2
qcom = 3
def process_msg(laikad, gnss_msg, mono_time, block=False):
# TODO: Understand and use remaining unknown constellations
if gnss_msg.which() == "drMeasurementReport":
if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']:
return None
if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max:
# gpsWeek 65535 is received rarely from quectel, this cannot be
# passed to GnssMeasurements's gpsWeek (Int16)
return None
return laikad.process_gnss_msg(gnss_msg, mono_time, block=block)
def clear_tmp_cache():
if os.path.exists(DOWNLOADS_CACHE_FOLDER):
shutil.rmtree(DOWNLOADS_CACHE_FOLDER)
os.mkdir(DOWNLOADS_CACHE_FOLDER)
def main(sm=None, pm=None, qc=None):
def main(sm=None, pm=None):
#clear_tmp_cache()
use_qcom = not Params().get_bool("UbloxAvailable", block=True)
if use_qcom or (qc is not None and qc):
raw_gnss_socket = "qcomGnss"
if use_qcom:
raw_name = "qcomGnss"
else:
raw_gnss_socket = "ubloxGnss"
raw_name = "ubloxGnss"
raw_gnss_sock = messaging.sub_sock(raw_name, conflate=False, timeout=1000)
if sm is None:
sm = messaging.SubMaster([raw_gnss_socket, 'clocks'])
sm = messaging.SubMaster(['clocks',])
if pm is None:
pm = messaging.PubMaster(['gnssMeasurements'])
# disable until set as main gps source, to better analyze startup time
use_internet = False #"LAIKAD_NO_INTERNET" not in os.environ
use_internet = False # "LAIKAD_NO_INTERNET" not in os.environ
replay = "REPLAY" in os.environ
if replay or "CI" in os.environ:
@@ -431,22 +435,17 @@ def main(sm=None, pm=None, qc=None):
laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom)
while True:
sm.update()
if sm.updated[raw_gnss_socket]:
gnss_msg = sm[raw_gnss_socket]
msg = process_msg(laikad, gnss_msg, sm.logMonoTime[raw_gnss_socket], replay)
if msg is None:
# TODO: beautify this, locationd needs a valid message
msg = messaging.new_message("gnssMeasurements")
pm.send('gnssMeasurements', msg)
for in_msg in messaging.drain_sock(raw_gnss_sock):
out_msg = laikad.process_gnss_msg(getattr(in_msg, raw_name), in_msg.logMonoTime, replay)
pm.send('gnssMeasurements', out_msg)
sm.update(0)
if not laikad.got_first_gnss_msg and sm.updated['clocks']:
clocks_msg = sm['clocks']
t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9))
if laikad.auto_fetch_navs:
laikad.fetch_navs(t, block=replay)
if __name__ == "__main__":
main()
selfdrive/locationd/locationd
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/car.cpp
+342 -342
View File
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
void err_fun(double *nom_x, double *delta_x, double *out_559111814741085424) {
out_559111814741085424[0] = delta_x[0] + nom_x[0];
out_559111814741085424[1] = delta_x[1] + nom_x[1];
out_559111814741085424[2] = delta_x[2] + nom_x[2];
out_559111814741085424[3] = delta_x[3] + nom_x[3];
out_559111814741085424[4] = delta_x[4] + nom_x[4];
out_559111814741085424[5] = delta_x[5] + nom_x[5];
out_559111814741085424[6] = delta_x[6] + nom_x[6];
out_559111814741085424[7] = delta_x[7] + nom_x[7];
out_559111814741085424[8] = delta_x[8] + nom_x[8];
void err_fun(double *nom_x, double *delta_x, double *out_5634330020296848070) {
out_5634330020296848070[0] = delta_x[0] + nom_x[0];
out_5634330020296848070[1] = delta_x[1] + nom_x[1];
out_5634330020296848070[2] = delta_x[2] + nom_x[2];
out_5634330020296848070[3] = delta_x[3] + nom_x[3];
out_5634330020296848070[4] = delta_x[4] + nom_x[4];
out_5634330020296848070[5] = delta_x[5] + nom_x[5];
out_5634330020296848070[6] = delta_x[6] + nom_x[6];
out_5634330020296848070[7] = delta_x[7] + nom_x[7];
out_5634330020296848070[8] = delta_x[8] + nom_x[8];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_8525802223358923727) {
out_8525802223358923727[0] = -nom_x[0] + true_x[0];
out_8525802223358923727[1] = -nom_x[1] + true_x[1];
out_8525802223358923727[2] = -nom_x[2] + true_x[2];
out_8525802223358923727[3] = -nom_x[3] + true_x[3];
out_8525802223358923727[4] = -nom_x[4] + true_x[4];
out_8525802223358923727[5] = -nom_x[5] + true_x[5];
out_8525802223358923727[6] = -nom_x[6] + true_x[6];
out_8525802223358923727[7] = -nom_x[7] + true_x[7];
out_8525802223358923727[8] = -nom_x[8] + true_x[8];
void inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281) {
out_1061559027503315281[0] = -nom_x[0] + true_x[0];
out_1061559027503315281[1] = -nom_x[1] + true_x[1];
out_1061559027503315281[2] = -nom_x[2] + true_x[2];
out_1061559027503315281[3] = -nom_x[3] + true_x[3];
out_1061559027503315281[4] = -nom_x[4] + true_x[4];
out_1061559027503315281[5] = -nom_x[5] + true_x[5];
out_1061559027503315281[6] = -nom_x[6] + true_x[6];
out_1061559027503315281[7] = -nom_x[7] + true_x[7];
out_1061559027503315281[8] = -nom_x[8] + true_x[8];
}
void H_mod_fun(double *state, double *out_6521334698987505192) {
out_6521334698987505192[0] = 1.0;
out_6521334698987505192[1] = 0;
out_6521334698987505192[2] = 0;
out_6521334698987505192[3] = 0;
out_6521334698987505192[4] = 0;
out_6521334698987505192[5] = 0;
out_6521334698987505192[6] = 0;
out_6521334698987505192[7] = 0;
out_6521334698987505192[8] = 0;
out_6521334698987505192[9] = 0;
out_6521334698987505192[10] = 1.0;
out_6521334698987505192[11] = 0;
out_6521334698987505192[12] = 0;
out_6521334698987505192[13] = 0;
out_6521334698987505192[14] = 0;
out_6521334698987505192[15] = 0;
out_6521334698987505192[16] = 0;
out_6521334698987505192[17] = 0;
out_6521334698987505192[18] = 0;
out_6521334698987505192[19] = 0;
out_6521334698987505192[20] = 1.0;
out_6521334698987505192[21] = 0;
out_6521334698987505192[22] = 0;
out_6521334698987505192[23] = 0;
out_6521334698987505192[24] = 0;
out_6521334698987505192[25] = 0;
out_6521334698987505192[26] = 0;
out_6521334698987505192[27] = 0;
out_6521334698987505192[28] = 0;
out_6521334698987505192[29] = 0;
out_6521334698987505192[30] = 1.0;
out_6521334698987505192[31] = 0;
out_6521334698987505192[32] = 0;
out_6521334698987505192[33] = 0;
out_6521334698987505192[34] = 0;
out_6521334698987505192[35] = 0;
out_6521334698987505192[36] = 0;
out_6521334698987505192[37] = 0;
out_6521334698987505192[38] = 0;
out_6521334698987505192[39] = 0;
out_6521334698987505192[40] = 1.0;
out_6521334698987505192[41] = 0;
out_6521334698987505192[42] = 0;
out_6521334698987505192[43] = 0;
out_6521334698987505192[44] = 0;
out_6521334698987505192[45] = 0;
out_6521334698987505192[46] = 0;
out_6521334698987505192[47] = 0;
out_6521334698987505192[48] = 0;
out_6521334698987505192[49] = 0;
out_6521334698987505192[50] = 1.0;
out_6521334698987505192[51] = 0;
out_6521334698987505192[52] = 0;
out_6521334698987505192[53] = 0;
out_6521334698987505192[54] = 0;
out_6521334698987505192[55] = 0;
out_6521334698987505192[56] = 0;
out_6521334698987505192[57] = 0;
out_6521334698987505192[58] = 0;
out_6521334698987505192[59] = 0;
out_6521334698987505192[60] = 1.0;
out_6521334698987505192[61] = 0;
out_6521334698987505192[62] = 0;
out_6521334698987505192[63] = 0;
out_6521334698987505192[64] = 0;
out_6521334698987505192[65] = 0;
out_6521334698987505192[66] = 0;
out_6521334698987505192[67] = 0;
out_6521334698987505192[68] = 0;
out_6521334698987505192[69] = 0;
out_6521334698987505192[70] = 1.0;
out_6521334698987505192[71] = 0;
out_6521334698987505192[72] = 0;
out_6521334698987505192[73] = 0;
out_6521334698987505192[74] = 0;
out_6521334698987505192[75] = 0;
out_6521334698987505192[76] = 0;
out_6521334698987505192[77] = 0;
out_6521334698987505192[78] = 0;
out_6521334698987505192[79] = 0;
out_6521334698987505192[80] = 1.0;
void H_mod_fun(double *state, double *out_1567569310651922730) {
out_1567569310651922730[0] = 1.0;
out_1567569310651922730[1] = 0;
out_1567569310651922730[2] = 0;
out_1567569310651922730[3] = 0;
out_1567569310651922730[4] = 0;
out_1567569310651922730[5] = 0;
out_1567569310651922730[6] = 0;
out_1567569310651922730[7] = 0;
out_1567569310651922730[8] = 0;
out_1567569310651922730[9] = 0;
out_1567569310651922730[10] = 1.0;
out_1567569310651922730[11] = 0;
out_1567569310651922730[12] = 0;
out_1567569310651922730[13] = 0;
out_1567569310651922730[14] = 0;
out_1567569310651922730[15] = 0;
out_1567569310651922730[16] = 0;
out_1567569310651922730[17] = 0;
out_1567569310651922730[18] = 0;
out_1567569310651922730[19] = 0;
out_1567569310651922730[20] = 1.0;
out_1567569310651922730[21] = 0;
out_1567569310651922730[22] = 0;
out_1567569310651922730[23] = 0;
out_1567569310651922730[24] = 0;
out_1567569310651922730[25] = 0;
out_1567569310651922730[26] = 0;
out_1567569310651922730[27] = 0;
out_1567569310651922730[28] = 0;
out_1567569310651922730[29] = 0;
out_1567569310651922730[30] = 1.0;
out_1567569310651922730[31] = 0;
out_1567569310651922730[32] = 0;
out_1567569310651922730[33] = 0;
out_1567569310651922730[34] = 0;
out_1567569310651922730[35] = 0;
out_1567569310651922730[36] = 0;
out_1567569310651922730[37] = 0;
out_1567569310651922730[38] = 0;
out_1567569310651922730[39] = 0;
out_1567569310651922730[40] = 1.0;
out_1567569310651922730[41] = 0;
out_1567569310651922730[42] = 0;
out_1567569310651922730[43] = 0;
out_1567569310651922730[44] = 0;
out_1567569310651922730[45] = 0;
out_1567569310651922730[46] = 0;
out_1567569310651922730[47] = 0;
out_1567569310651922730[48] = 0;
out_1567569310651922730[49] = 0;
out_1567569310651922730[50] = 1.0;
out_1567569310651922730[51] = 0;
out_1567569310651922730[52] = 0;
out_1567569310651922730[53] = 0;
out_1567569310651922730[54] = 0;
out_1567569310651922730[55] = 0;
out_1567569310651922730[56] = 0;
out_1567569310651922730[57] = 0;
out_1567569310651922730[58] = 0;
out_1567569310651922730[59] = 0;
out_1567569310651922730[60] = 1.0;
out_1567569310651922730[61] = 0;
out_1567569310651922730[62] = 0;
out_1567569310651922730[63] = 0;
out_1567569310651922730[64] = 0;
out_1567569310651922730[65] = 0;
out_1567569310651922730[66] = 0;
out_1567569310651922730[67] = 0;
out_1567569310651922730[68] = 0;
out_1567569310651922730[69] = 0;
out_1567569310651922730[70] = 1.0;
out_1567569310651922730[71] = 0;
out_1567569310651922730[72] = 0;
out_1567569310651922730[73] = 0;
out_1567569310651922730[74] = 0;
out_1567569310651922730[75] = 0;
out_1567569310651922730[76] = 0;
out_1567569310651922730[77] = 0;
out_1567569310651922730[78] = 0;
out_1567569310651922730[79] = 0;
out_1567569310651922730[80] = 1.0;
}
void f_fun(double *state, double dt, double *out_5157815911269073647) {
out_5157815911269073647[0] = state[0];
out_5157815911269073647[1] = state[1];
out_5157815911269073647[2] = state[2];
out_5157815911269073647[3] = state[3];
out_5157815911269073647[4] = state[4];
out_5157815911269073647[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_5157815911269073647[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_5157815911269073647[7] = state[7];
out_5157815911269073647[8] = state[8];
void f_fun(double *state, double dt, double *out_1208381054458531187) {
out_1208381054458531187[0] = state[0];
out_1208381054458531187[1] = state[1];
out_1208381054458531187[2] = state[2];
out_1208381054458531187[3] = state[3];
out_1208381054458531187[4] = state[4];
out_1208381054458531187[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_1208381054458531187[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_1208381054458531187[7] = state[7];
out_1208381054458531187[8] = state[8];
}
void F_fun(double *state, double dt, double *out_4341564588224271600) {
out_4341564588224271600[0] = 1;
out_4341564588224271600[1] = 0;
out_4341564588224271600[2] = 0;
out_4341564588224271600[3] = 0;
out_4341564588224271600[4] = 0;
out_4341564588224271600[5] = 0;
out_4341564588224271600[6] = 0;
out_4341564588224271600[7] = 0;
out_4341564588224271600[8] = 0;
out_4341564588224271600[9] = 0;
out_4341564588224271600[10] = 1;
out_4341564588224271600[11] = 0;
out_4341564588224271600[12] = 0;
out_4341564588224271600[13] = 0;
out_4341564588224271600[14] = 0;
out_4341564588224271600[15] = 0;
out_4341564588224271600[16] = 0;
out_4341564588224271600[17] = 0;
out_4341564588224271600[18] = 0;
out_4341564588224271600[19] = 0;
out_4341564588224271600[20] = 1;
out_4341564588224271600[21] = 0;
out_4341564588224271600[22] = 0;
out_4341564588224271600[23] = 0;
out_4341564588224271600[24] = 0;
out_4341564588224271600[25] = 0;
out_4341564588224271600[26] = 0;
out_4341564588224271600[27] = 0;
out_4341564588224271600[28] = 0;
out_4341564588224271600[29] = 0;
out_4341564588224271600[30] = 1;
out_4341564588224271600[31] = 0;
out_4341564588224271600[32] = 0;
out_4341564588224271600[33] = 0;
out_4341564588224271600[34] = 0;
out_4341564588224271600[35] = 0;
out_4341564588224271600[36] = 0;
out_4341564588224271600[37] = 0;
out_4341564588224271600[38] = 0;
out_4341564588224271600[39] = 0;
out_4341564588224271600[40] = 1;
out_4341564588224271600[41] = 0;
out_4341564588224271600[42] = 0;
out_4341564588224271600[43] = 0;
out_4341564588224271600[44] = 0;
out_4341564588224271600[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_4341564588224271600[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_4341564588224271600[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_4341564588224271600[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_4341564588224271600[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_4341564588224271600[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_4341564588224271600[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_4341564588224271600[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_4341564588224271600[53] = -9.8000000000000007*dt;
out_4341564588224271600[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_4341564588224271600[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_4341564588224271600[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_4341564588224271600[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_4341564588224271600[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_4341564588224271600[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_4341564588224271600[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_4341564588224271600[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_4341564588224271600[62] = 0;
out_4341564588224271600[63] = 0;
out_4341564588224271600[64] = 0;
out_4341564588224271600[65] = 0;
out_4341564588224271600[66] = 0;
out_4341564588224271600[67] = 0;
out_4341564588224271600[68] = 0;
out_4341564588224271600[69] = 0;
out_4341564588224271600[70] = 1;
out_4341564588224271600[71] = 0;
out_4341564588224271600[72] = 0;
out_4341564588224271600[73] = 0;
out_4341564588224271600[74] = 0;
out_4341564588224271600[75] = 0;
out_4341564588224271600[76] = 0;
out_4341564588224271600[77] = 0;
out_4341564588224271600[78] = 0;
out_4341564588224271600[79] = 0;
out_4341564588224271600[80] = 1;
void F_fun(double *state, double dt, double *out_8824004521572161416) {
out_8824004521572161416[0] = 1;
out_8824004521572161416[1] = 0;
out_8824004521572161416[2] = 0;
out_8824004521572161416[3] = 0;
out_8824004521572161416[4] = 0;
out_8824004521572161416[5] = 0;
out_8824004521572161416[6] = 0;
out_8824004521572161416[7] = 0;
out_8824004521572161416[8] = 0;
out_8824004521572161416[9] = 0;
out_8824004521572161416[10] = 1;
out_8824004521572161416[11] = 0;
out_8824004521572161416[12] = 0;
out_8824004521572161416[13] = 0;
out_8824004521572161416[14] = 0;
out_8824004521572161416[15] = 0;
out_8824004521572161416[16] = 0;
out_8824004521572161416[17] = 0;
out_8824004521572161416[18] = 0;
out_8824004521572161416[19] = 0;
out_8824004521572161416[20] = 1;
out_8824004521572161416[21] = 0;
out_8824004521572161416[22] = 0;
out_8824004521572161416[23] = 0;
out_8824004521572161416[24] = 0;
out_8824004521572161416[25] = 0;
out_8824004521572161416[26] = 0;
out_8824004521572161416[27] = 0;
out_8824004521572161416[28] = 0;
out_8824004521572161416[29] = 0;
out_8824004521572161416[30] = 1;
out_8824004521572161416[31] = 0;
out_8824004521572161416[32] = 0;
out_8824004521572161416[33] = 0;
out_8824004521572161416[34] = 0;
out_8824004521572161416[35] = 0;
out_8824004521572161416[36] = 0;
out_8824004521572161416[37] = 0;
out_8824004521572161416[38] = 0;
out_8824004521572161416[39] = 0;
out_8824004521572161416[40] = 1;
out_8824004521572161416[41] = 0;
out_8824004521572161416[42] = 0;
out_8824004521572161416[43] = 0;
out_8824004521572161416[44] = 0;
out_8824004521572161416[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_8824004521572161416[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_8824004521572161416[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_8824004521572161416[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_8824004521572161416[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_8824004521572161416[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_8824004521572161416[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_8824004521572161416[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_8824004521572161416[53] = -9.8000000000000007*dt;
out_8824004521572161416[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_8824004521572161416[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_8824004521572161416[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_8824004521572161416[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_8824004521572161416[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_8824004521572161416[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_8824004521572161416[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_8824004521572161416[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_8824004521572161416[62] = 0;
out_8824004521572161416[63] = 0;
out_8824004521572161416[64] = 0;
out_8824004521572161416[65] = 0;
out_8824004521572161416[66] = 0;
out_8824004521572161416[67] = 0;
out_8824004521572161416[68] = 0;
out_8824004521572161416[69] = 0;
out_8824004521572161416[70] = 1;
out_8824004521572161416[71] = 0;
out_8824004521572161416[72] = 0;
out_8824004521572161416[73] = 0;
out_8824004521572161416[74] = 0;
out_8824004521572161416[75] = 0;
out_8824004521572161416[76] = 0;
out_8824004521572161416[77] = 0;
out_8824004521572161416[78] = 0;
out_8824004521572161416[79] = 0;
out_8824004521572161416[80] = 1;
}
void h_25(double *state, double *unused, double *out_8809968363646297090) {
out_8809968363646297090[0] = state[6];
void h_25(double *state, double *unused, double *out_1789446864250922111) {
out_1789446864250922111[0] = state[6];
}
void H_25(double *state, double *unused, double *out_1959634654218019643) {
out_1959634654218019643[0] = 0;
out_1959634654218019643[1] = 0;
out_1959634654218019643[2] = 0;
out_1959634654218019643[3] = 0;
out_1959634654218019643[4] = 0;
out_1959634654218019643[5] = 0;
out_1959634654218019643[6] = 1;
out_1959634654218019643[7] = 0;
out_1959634654218019643[8] = 0;
void H_25(double *state, double *unused, double *out_9129886545195087354) {
out_9129886545195087354[0] = 0;
out_9129886545195087354[1] = 0;
out_9129886545195087354[2] = 0;
out_9129886545195087354[3] = 0;
out_9129886545195087354[4] = 0;
out_9129886545195087354[5] = 0;
out_9129886545195087354[6] = 1;
out_9129886545195087354[7] = 0;
out_9129886545195087354[8] = 0;
}
void h_24(double *state, double *unused, double *out_8028835485487409495) {
out_8028835485487409495[0] = state[4];
out_8028835485487409495[1] = state[5];
void h_24(double *state, double *unused, double *out_3299160385053087312) {
out_3299160385053087312[0] = state[4];
out_3299160385053087312[1] = state[5];
}
void H_24(double *state, double *unused, double *out_2786874672748489391) {
out_2786874672748489391[0] = 0;
out_2786874672748489391[1] = 0;
out_2786874672748489391[2] = 0;
out_2786874672748489391[3] = 0;
out_2786874672748489391[4] = 1;
out_2786874672748489391[5] = 0;
out_2786874672748489391[6] = 0;
out_2786874672748489391[7] = 0;
out_2786874672748489391[8] = 0;
out_2786874672748489391[9] = 0;
out_2786874672748489391[10] = 0;
out_2786874672748489391[11] = 0;
out_2786874672748489391[12] = 0;
out_2786874672748489391[13] = 0;
out_2786874672748489391[14] = 1;
out_2786874672748489391[15] = 0;
out_2786874672748489391[16] = 0;
out_2786874672748489391[17] = 0;
void H_24(double *state, double *unused, double *out_7144207929508964696) {
out_7144207929508964696[0] = 0;
out_7144207929508964696[1] = 0;
out_7144207929508964696[2] = 0;
out_7144207929508964696[3] = 0;
out_7144207929508964696[4] = 1;
out_7144207929508964696[5] = 0;
out_7144207929508964696[6] = 0;
out_7144207929508964696[7] = 0;
out_7144207929508964696[8] = 0;
out_7144207929508964696[9] = 0;
out_7144207929508964696[10] = 0;
out_7144207929508964696[11] = 0;
out_7144207929508964696[12] = 0;
out_7144207929508964696[13] = 0;
out_7144207929508964696[14] = 1;
out_7144207929508964696[15] = 0;
out_7144207929508964696[16] = 0;
out_7144207929508964696[17] = 0;
}
void h_30(double *state, double *unused, double *out_8967155031997426235) {
out_8967155031997426235[0] = state[4];
void h_30(double *state, double *unused, double *out_7348270420432929071) {
out_7348270420432929071[0] = state[4];
}
void H_30(double *state, double *unused, double *out_4477967612725268270) {
out_4477967612725268270[0] = 0;
out_4477967612725268270[1] = 0;
out_4477967612725268270[2] = 0;
out_4477967612725268270[3] = 0;
out_4477967612725268270[4] = 1;
out_4477967612725268270[5] = 0;
out_4477967612725268270[6] = 0;
out_4477967612725268270[7] = 0;
out_4477967612725268270[8] = 0;
void H_30(double *state, double *unused, double *out_6611553586687838727) {
out_6611553586687838727[0] = 0;
out_6611553586687838727[1] = 0;
out_6611553586687838727[2] = 0;
out_6611553586687838727[3] = 0;
out_6611553586687838727[4] = 1;
out_6611553586687838727[5] = 0;
out_6611553586687838727[6] = 0;
out_6611553586687838727[7] = 0;
out_6611553586687838727[8] = 0;
}
void h_26(double *state, double *unused, double *out_4004775034316133003) {
out_4004775034316133003[0] = state[7];
void h_26(double *state, double *unused, double *out_6096800385615776900) {
out_6096800385615776900[0] = state[7];
}
void H_26(double *state, double *unused, double *out_5264160623978820244) {
out_5264160623978820244[0] = 0;
out_5264160623978820244[1] = 0;
out_5264160623978820244[2] = 0;
out_5264160623978820244[3] = 0;
out_5264160623978820244[4] = 0;
out_5264160623978820244[5] = 0;
out_5264160623978820244[6] = 0;
out_5264160623978820244[7] = 1;
out_5264160623978820244[8] = 0;
void H_26(double *state, double *unused, double *out_5575354209640408038) {
out_5575354209640408038[0] = 0;
out_5575354209640408038[1] = 0;
out_5575354209640408038[2] = 0;
out_5575354209640408038[3] = 0;
out_5575354209640408038[4] = 0;
out_5575354209640408038[5] = 0;
out_5575354209640408038[6] = 0;
out_5575354209640408038[7] = 1;
out_5575354209640408038[8] = 0;
}
void h_27(double *state, double *unused, double *out_6824223606793401143) {
out_6824223606793401143[0] = state[3];
void h_27(double *state, double *unused, double *out_1247339187193410574) {
out_1247339187193410574[0] = state[3];
}
void H_27(double *state, double *unused, double *out_2303204300924843359) {
out_2303204300924843359[0] = 0;
out_2303204300924843359[1] = 0;
out_2303204300924843359[2] = 0;
out_2303204300924843359[3] = 1;
out_2303204300924843359[4] = 0;
out_2303204300924843359[5] = 0;
out_2303204300924843359[6] = 0;
out_2303204300924843359[7] = 0;
out_2303204300924843359[8] = 0;
void H_27(double *state, double *unused, double *out_2614397886586431153) {
out_2614397886586431153[0] = 0;
out_2614397886586431153[1] = 0;
out_2614397886586431153[2] = 0;
out_2614397886586431153[3] = 1;
out_2614397886586431153[4] = 0;
out_2614397886586431153[5] = 0;
out_2614397886586431153[6] = 0;
out_2614397886586431153[7] = 0;
out_2614397886586431153[8] = 0;
}
void h_29(double *state, double *unused, double *out_53388380443050207) {
out_53388380443050207[0] = state[1];
void h_29(double *state, double *unused, double *out_1705540549351972236) {
out_1705540549351972236[0] = state[1];
}
void H_29(double *state, double *unused, double *out_4988198957039660454) {
out_4988198957039660454[0] = 0;
out_4988198957039660454[1] = 1;
out_4988198957039660454[2] = 0;
out_4988198957039660454[3] = 0;
out_4988198957039660454[4] = 0;
out_4988198957039660454[5] = 0;
out_4988198957039660454[6] = 0;
out_4988198957039660454[7] = 0;
out_4988198957039660454[8] = 0;
void H_29(double *state, double *unused, double *out_5299392542701248248) {
out_5299392542701248248[0] = 0;
out_5299392542701248248[1] = 1;
out_5299392542701248248[2] = 0;
out_5299392542701248248[3] = 0;
out_5299392542701248248[4] = 0;
out_5299392542701248248[5] = 0;
out_5299392542701248248[6] = 0;
out_5299392542701248248[7] = 0;
out_5299392542701248248[8] = 0;
}
void h_28(double *state, double *unused, double *out_6620847199255304828) {
out_6620847199255304828[0] = state[0];
void h_28(double *state, double *unused, double *out_4170451758575142464) {
out_4170451758575142464[0] = state[0];
}
void H_28(double *state, double *unused, double *out_94200060029870120) {
out_94200060029870120[0] = 1;
out_94200060029870120[1] = 0;
out_94200060029870120[2] = 0;
out_94200060029870120[3] = 0;
out_94200060029870120[4] = 0;
out_94200060029870120[5] = 0;
out_94200060029870120[6] = 0;
out_94200060029870120[7] = 0;
out_94200060029870120[8] = 0;
void H_28(double *state, double *unused, double *out_7263022814266574499) {
out_7263022814266574499[0] = 1;
out_7263022814266574499[1] = 0;
out_7263022814266574499[2] = 0;
out_7263022814266574499[3] = 0;
out_7263022814266574499[4] = 0;
out_7263022814266574499[5] = 0;
out_7263022814266574499[6] = 0;
out_7263022814266574499[7] = 0;
out_7263022814266574499[8] = 0;
}
void h_31(double *state, double *unused, double *out_9085162425930802979) {
out_9085162425930802979[0] = state[8];
void h_31(double *state, double *unused, double *out_1514252801966416222) {
out_1514252801966416222[0] = state[8];
}
void H_31(double *state, double *unused, double *out_4637952521745468768) {
out_4637952521745468768[0] = 0;
out_4637952521745468768[1] = 0;
out_4637952521745468768[2] = 0;
out_4637952521745468768[3] = 0;
out_4637952521745468768[4] = 0;
out_4637952521745468768[5] = 0;
out_4637952521745468768[6] = 0;
out_4637952521745468768[7] = 0;
out_4637952521745468768[8] = 1;
void H_31(double *state, double *unused, double *out_4949146107407056562) {
out_4949146107407056562[0] = 0;
out_4949146107407056562[1] = 0;
out_4949146107407056562[2] = 0;
out_4949146107407056562[3] = 0;
out_4949146107407056562[4] = 0;
out_4949146107407056562[5] = 0;
out_4949146107407056562[6] = 0;
out_4949146107407056562[7] = 0;
out_4949146107407056562[8] = 1;
}
#include <eigen3/Eigen/Dense>
#include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
}
void car_err_fun(double *nom_x, double *delta_x, double *out_559111814741085424) {
err_fun(nom_x, delta_x, out_559111814741085424);
void car_err_fun(double *nom_x, double *delta_x, double *out_5634330020296848070) {
err_fun(nom_x, delta_x, out_5634330020296848070);
}
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8525802223358923727) {
inv_err_fun(nom_x, true_x, out_8525802223358923727);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281) {
inv_err_fun(nom_x, true_x, out_1061559027503315281);
}
void car_H_mod_fun(double *state, double *out_6521334698987505192) {
H_mod_fun(state, out_6521334698987505192);
void car_H_mod_fun(double *state, double *out_1567569310651922730) {
H_mod_fun(state, out_1567569310651922730);
}
void car_f_fun(double *state, double dt, double *out_5157815911269073647) {
f_fun(state, dt, out_5157815911269073647);
void car_f_fun(double *state, double dt, double *out_1208381054458531187) {
f_fun(state, dt, out_1208381054458531187);
}
void car_F_fun(double *state, double dt, double *out_4341564588224271600) {
F_fun(state, dt, out_4341564588224271600);
void car_F_fun(double *state, double dt, double *out_8824004521572161416) {
F_fun(state, dt, out_8824004521572161416);
}
void car_h_25(double *state, double *unused, double *out_8809968363646297090) {
h_25(state, unused, out_8809968363646297090);
void car_h_25(double *state, double *unused, double *out_1789446864250922111) {
h_25(state, unused, out_1789446864250922111);
}
void car_H_25(double *state, double *unused, double *out_1959634654218019643) {
H_25(state, unused, out_1959634654218019643);
void car_H_25(double *state, double *unused, double *out_9129886545195087354) {
H_25(state, unused, out_9129886545195087354);
}
void car_h_24(double *state, double *unused, double *out_8028835485487409495) {
h_24(state, unused, out_8028835485487409495);
void car_h_24(double *state, double *unused, double *out_3299160385053087312) {
h_24(state, unused, out_3299160385053087312);
}
void car_H_24(double *state, double *unused, double *out_2786874672748489391) {
H_24(state, unused, out_2786874672748489391);
void car_H_24(double *state, double *unused, double *out_7144207929508964696) {
H_24(state, unused, out_7144207929508964696);
}
void car_h_30(double *state, double *unused, double *out_8967155031997426235) {
h_30(state, unused, out_8967155031997426235);
void car_h_30(double *state, double *unused, double *out_7348270420432929071) {
h_30(state, unused, out_7348270420432929071);
}
void car_H_30(double *state, double *unused, double *out_4477967612725268270) {
H_30(state, unused, out_4477967612725268270);
void car_H_30(double *state, double *unused, double *out_6611553586687838727) {
H_30(state, unused, out_6611553586687838727);
}
void car_h_26(double *state, double *unused, double *out_4004775034316133003) {
h_26(state, unused, out_4004775034316133003);
void car_h_26(double *state, double *unused, double *out_6096800385615776900) {
h_26(state, unused, out_6096800385615776900);
}
void car_H_26(double *state, double *unused, double *out_5264160623978820244) {
H_26(state, unused, out_5264160623978820244);
void car_H_26(double *state, double *unused, double *out_5575354209640408038) {
H_26(state, unused, out_5575354209640408038);
}
void car_h_27(double *state, double *unused, double *out_6824223606793401143) {
h_27(state, unused, out_6824223606793401143);
void car_h_27(double *state, double *unused, double *out_1247339187193410574) {
h_27(state, unused, out_1247339187193410574);
}
void car_H_27(double *state, double *unused, double *out_2303204300924843359) {
H_27(state, unused, out_2303204300924843359);
void car_H_27(double *state, double *unused, double *out_2614397886586431153) {
H_27(state, unused, out_2614397886586431153);
}
void car_h_29(double *state, double *unused, double *out_53388380443050207) {
h_29(state, unused, out_53388380443050207);
void car_h_29(double *state, double *unused, double *out_1705540549351972236) {
h_29(state, unused, out_1705540549351972236);
}
void car_H_29(double *state, double *unused, double *out_4988198957039660454) {
H_29(state, unused, out_4988198957039660454);
void car_H_29(double *state, double *unused, double *out_5299392542701248248) {
H_29(state, unused, out_5299392542701248248);
}
void car_h_28(double *state, double *unused, double *out_6620847199255304828) {
h_28(state, unused, out_6620847199255304828);
void car_h_28(double *state, double *unused, double *out_4170451758575142464) {
h_28(state, unused, out_4170451758575142464);
}
void car_H_28(double *state, double *unused, double *out_94200060029870120) {
H_28(state, unused, out_94200060029870120);
void car_H_28(double *state, double *unused, double *out_7263022814266574499) {
H_28(state, unused, out_7263022814266574499);
}
void car_h_31(double *state, double *unused, double *out_9085162425930802979) {
h_31(state, unused, out_9085162425930802979);
void car_h_31(double *state, double *unused, double *out_1514252801966416222) {
h_31(state, unused, out_1514252801966416222);
}
void car_H_31(double *state, double *unused, double *out_4637952521745468768) {
H_31(state, unused, out_4637952521745468768);
void car_H_31(double *state, double *unused, double *out_4949146107407056562) {
H_31(state, unused, out_4949146107407056562);
}
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
selfdrive/locationd/models/generated/car.h
+21 -21
View File
@@ -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_559111814741085424);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8525802223358923727);
void car_H_mod_fun(double *state, double *out_6521334698987505192);
void car_f_fun(double *state, double dt, double *out_5157815911269073647);
void car_F_fun(double *state, double dt, double *out_4341564588224271600);
void car_h_25(double *state, double *unused, double *out_8809968363646297090);
void car_H_25(double *state, double *unused, double *out_1959634654218019643);
void car_h_24(double *state, double *unused, double *out_8028835485487409495);
void car_H_24(double *state, double *unused, double *out_2786874672748489391);
void car_h_30(double *state, double *unused, double *out_8967155031997426235);
void car_H_30(double *state, double *unused, double *out_4477967612725268270);
void car_h_26(double *state, double *unused, double *out_4004775034316133003);
void car_H_26(double *state, double *unused, double *out_5264160623978820244);
void car_h_27(double *state, double *unused, double *out_6824223606793401143);
void car_H_27(double *state, double *unused, double *out_2303204300924843359);
void car_h_29(double *state, double *unused, double *out_53388380443050207);
void car_H_29(double *state, double *unused, double *out_4988198957039660454);
void car_h_28(double *state, double *unused, double *out_6620847199255304828);
void car_H_28(double *state, double *unused, double *out_94200060029870120);
void car_h_31(double *state, double *unused, double *out_9085162425930802979);
void car_H_31(double *state, double *unused, double *out_4637952521745468768);
void car_err_fun(double *nom_x, double *delta_x, double *out_5634330020296848070);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_1061559027503315281);
void car_H_mod_fun(double *state, double *out_1567569310651922730);
void car_f_fun(double *state, double dt, double *out_1208381054458531187);
void car_F_fun(double *state, double dt, double *out_8824004521572161416);
void car_h_25(double *state, double *unused, double *out_1789446864250922111);
void car_H_25(double *state, double *unused, double *out_9129886545195087354);
void car_h_24(double *state, double *unused, double *out_3299160385053087312);
void car_H_24(double *state, double *unused, double *out_7144207929508964696);
void car_h_30(double *state, double *unused, double *out_7348270420432929071);
void car_H_30(double *state, double *unused, double *out_6611553586687838727);
void car_h_26(double *state, double *unused, double *out_6096800385615776900);
void car_H_26(double *state, double *unused, double *out_5575354209640408038);
void car_h_27(double *state, double *unused, double *out_1247339187193410574);
void car_H_27(double *state, double *unused, double *out_2614397886586431153);
void car_h_29(double *state, double *unused, double *out_1705540549351972236);
void car_H_29(double *state, double *unused, double *out_5299392542701248248);
void car_h_28(double *state, double *unused, double *out_4170451758575142464);
void car_H_28(double *state, double *unused, double *out_7263022814266574499);
void car_h_31(double *state, double *unused, double *out_1514252801966416222);
void car_H_31(double *state, double *unused, double *out_4949146107407056562);
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);
selfdrive/locationd/models/generated/gnss.cpp
+362 -362
View File
@@ -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_654910612427940843) {
out_654910612427940843[0] = delta_x[0] + nom_x[0];
out_654910612427940843[1] = delta_x[1] + nom_x[1];
out_654910612427940843[2] = delta_x[2] + nom_x[2];
out_654910612427940843[3] = delta_x[3] + nom_x[3];
out_654910612427940843[4] = delta_x[4] + nom_x[4];
out_654910612427940843[5] = delta_x[5] + nom_x[5];
out_654910612427940843[6] = delta_x[6] + nom_x[6];
out_654910612427940843[7] = delta_x[7] + nom_x[7];
out_654910612427940843[8] = delta_x[8] + nom_x[8];
out_654910612427940843[9] = delta_x[9] + nom_x[9];
out_654910612427940843[10] = delta_x[10] + nom_x[10];
void err_fun(double *nom_x, double *delta_x, double *out_8218999594846759964) {
out_8218999594846759964[0] = delta_x[0] + nom_x[0];
out_8218999594846759964[1] = delta_x[1] + nom_x[1];
out_8218999594846759964[2] = delta_x[2] + nom_x[2];
out_8218999594846759964[3] = delta_x[3] + nom_x[3];
out_8218999594846759964[4] = delta_x[4] + nom_x[4];
out_8218999594846759964[5] = delta_x[5] + nom_x[5];
out_8218999594846759964[6] = delta_x[6] + nom_x[6];
out_8218999594846759964[7] = delta_x[7] + nom_x[7];
out_8218999594846759964[8] = delta_x[8] + nom_x[8];
out_8218999594846759964[9] = delta_x[9] + nom_x[9];
out_8218999594846759964[10] = delta_x[10] + nom_x[10];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_6637724801134549563) {
out_6637724801134549563[0] = -nom_x[0] + true_x[0];
out_6637724801134549563[1] = -nom_x[1] + true_x[1];
out_6637724801134549563[2] = -nom_x[2] + true_x[2];
out_6637724801134549563[3] = -nom_x[3] + true_x[3];
out_6637724801134549563[4] = -nom_x[4] + true_x[4];
out_6637724801134549563[5] = -nom_x[5] + true_x[5];
out_6637724801134549563[6] = -nom_x[6] + true_x[6];
out_6637724801134549563[7] = -nom_x[7] + true_x[7];
out_6637724801134549563[8] = -nom_x[8] + true_x[8];
out_6637724801134549563[9] = -nom_x[9] + true_x[9];
out_6637724801134549563[10] = -nom_x[10] + true_x[10];
void inv_err_fun(double *nom_x, double *true_x, double *out_6004490369099356165) {
out_6004490369099356165[0] = -nom_x[0] + true_x[0];
out_6004490369099356165[1] = -nom_x[1] + true_x[1];
out_6004490369099356165[2] = -nom_x[2] + true_x[2];
out_6004490369099356165[3] = -nom_x[3] + true_x[3];
out_6004490369099356165[4] = -nom_x[4] + true_x[4];
out_6004490369099356165[5] = -nom_x[5] + true_x[5];
out_6004490369099356165[6] = -nom_x[6] + true_x[6];
out_6004490369099356165[7] = -nom_x[7] + true_x[7];
out_6004490369099356165[8] = -nom_x[8] + true_x[8];
out_6004490369099356165[9] = -nom_x[9] + true_x[9];
out_6004490369099356165[10] = -nom_x[10] + true_x[10];
}
void H_mod_fun(double *state, double *out_805386010827368873) {
out_805386010827368873[0] = 1.0;
out_805386010827368873[1] = 0;
out_805386010827368873[2] = 0;
out_805386010827368873[3] = 0;
out_805386010827368873[4] = 0;
out_805386010827368873[5] = 0;
out_805386010827368873[6] = 0;
out_805386010827368873[7] = 0;
out_805386010827368873[8] = 0;
out_805386010827368873[9] = 0;
out_805386010827368873[10] = 0;
out_805386010827368873[11] = 0;
out_805386010827368873[12] = 1.0;
out_805386010827368873[13] = 0;
out_805386010827368873[14] = 0;
out_805386010827368873[15] = 0;
out_805386010827368873[16] = 0;
out_805386010827368873[17] = 0;
out_805386010827368873[18] = 0;
out_805386010827368873[19] = 0;
out_805386010827368873[20] = 0;
out_805386010827368873[21] = 0;
out_805386010827368873[22] = 0;
out_805386010827368873[23] = 0;
out_805386010827368873[24] = 1.0;
out_805386010827368873[25] = 0;
out_805386010827368873[26] = 0;
out_805386010827368873[27] = 0;
out_805386010827368873[28] = 0;
out_805386010827368873[29] = 0;
out_805386010827368873[30] = 0;
out_805386010827368873[31] = 0;
out_805386010827368873[32] = 0;
out_805386010827368873[33] = 0;
out_805386010827368873[34] = 0;
out_805386010827368873[35] = 0;
out_805386010827368873[36] = 1.0;
out_805386010827368873[37] = 0;
out_805386010827368873[38] = 0;
out_805386010827368873[39] = 0;
out_805386010827368873[40] = 0;
out_805386010827368873[41] = 0;
out_805386010827368873[42] = 0;
out_805386010827368873[43] = 0;
out_805386010827368873[44] = 0;
out_805386010827368873[45] = 0;
out_805386010827368873[46] = 0;
out_805386010827368873[47] = 0;
out_805386010827368873[48] = 1.0;
out_805386010827368873[49] = 0;
out_805386010827368873[50] = 0;
out_805386010827368873[51] = 0;
out_805386010827368873[52] = 0;
out_805386010827368873[53] = 0;
out_805386010827368873[54] = 0;
out_805386010827368873[55] = 0;
out_805386010827368873[56] = 0;
out_805386010827368873[57] = 0;
out_805386010827368873[58] = 0;
out_805386010827368873[59] = 0;
out_805386010827368873[60] = 1.0;
out_805386010827368873[61] = 0;
out_805386010827368873[62] = 0;
out_805386010827368873[63] = 0;
out_805386010827368873[64] = 0;
out_805386010827368873[65] = 0;
out_805386010827368873[66] = 0;
out_805386010827368873[67] = 0;
out_805386010827368873[68] = 0;
out_805386010827368873[69] = 0;
out_805386010827368873[70] = 0;
out_805386010827368873[71] = 0;
out_805386010827368873[72] = 1.0;
out_805386010827368873[73] = 0;
out_805386010827368873[74] = 0;
out_805386010827368873[75] = 0;
out_805386010827368873[76] = 0;
out_805386010827368873[77] = 0;
out_805386010827368873[78] = 0;
out_805386010827368873[79] = 0;
out_805386010827368873[80] = 0;
out_805386010827368873[81] = 0;
out_805386010827368873[82] = 0;
out_805386010827368873[83] = 0;
out_805386010827368873[84] = 1.0;
out_805386010827368873[85] = 0;
out_805386010827368873[86] = 0;
out_805386010827368873[87] = 0;
out_805386010827368873[88] = 0;
out_805386010827368873[89] = 0;
out_805386010827368873[90] = 0;
out_805386010827368873[91] = 0;
out_805386010827368873[92] = 0;
out_805386010827368873[93] = 0;
out_805386010827368873[94] = 0;
out_805386010827368873[95] = 0;
out_805386010827368873[96] = 1.0;
out_805386010827368873[97] = 0;
out_805386010827368873[98] = 0;
out_805386010827368873[99] = 0;
out_805386010827368873[100] = 0;
out_805386010827368873[101] = 0;
out_805386010827368873[102] = 0;
out_805386010827368873[103] = 0;
out_805386010827368873[104] = 0;
out_805386010827368873[105] = 0;
out_805386010827368873[106] = 0;
out_805386010827368873[107] = 0;
out_805386010827368873[108] = 1.0;
out_805386010827368873[109] = 0;
out_805386010827368873[110] = 0;
out_805386010827368873[111] = 0;
out_805386010827368873[112] = 0;
out_805386010827368873[113] = 0;
out_805386010827368873[114] = 0;
out_805386010827368873[115] = 0;
out_805386010827368873[116] = 0;
out_805386010827368873[117] = 0;
out_805386010827368873[118] = 0;
out_805386010827368873[119] = 0;
out_805386010827368873[120] = 1.0;
void H_mod_fun(double *state, double *out_5465143194872640210) {
out_5465143194872640210[0] = 1.0;
out_5465143194872640210[1] = 0;
out_5465143194872640210[2] = 0;
out_5465143194872640210[3] = 0;
out_5465143194872640210[4] = 0;
out_5465143194872640210[5] = 0;
out_5465143194872640210[6] = 0;
out_5465143194872640210[7] = 0;
out_5465143194872640210[8] = 0;
out_5465143194872640210[9] = 0;
out_5465143194872640210[10] = 0;
out_5465143194872640210[11] = 0;
out_5465143194872640210[12] = 1.0;
out_5465143194872640210[13] = 0;
out_5465143194872640210[14] = 0;
out_5465143194872640210[15] = 0;
out_5465143194872640210[16] = 0;
out_5465143194872640210[17] = 0;
out_5465143194872640210[18] = 0;
out_5465143194872640210[19] = 0;
out_5465143194872640210[20] = 0;
out_5465143194872640210[21] = 0;
out_5465143194872640210[22] = 0;
out_5465143194872640210[23] = 0;
out_5465143194872640210[24] = 1.0;
out_5465143194872640210[25] = 0;
out_5465143194872640210[26] = 0;
out_5465143194872640210[27] = 0;
out_5465143194872640210[28] = 0;
out_5465143194872640210[29] = 0;
out_5465143194872640210[30] = 0;
out_5465143194872640210[31] = 0;
out_5465143194872640210[32] = 0;
out_5465143194872640210[33] = 0;
out_5465143194872640210[34] = 0;
out_5465143194872640210[35] = 0;
out_5465143194872640210[36] = 1.0;
out_5465143194872640210[37] = 0;
out_5465143194872640210[38] = 0;
out_5465143194872640210[39] = 0;
out_5465143194872640210[40] = 0;
out_5465143194872640210[41] = 0;
out_5465143194872640210[42] = 0;
out_5465143194872640210[43] = 0;
out_5465143194872640210[44] = 0;
out_5465143194872640210[45] = 0;
out_5465143194872640210[46] = 0;
out_5465143194872640210[47] = 0;
out_5465143194872640210[48] = 1.0;
out_5465143194872640210[49] = 0;
out_5465143194872640210[50] = 0;
out_5465143194872640210[51] = 0;
out_5465143194872640210[52] = 0;
out_5465143194872640210[53] = 0;
out_5465143194872640210[54] = 0;
out_5465143194872640210[55] = 0;
out_5465143194872640210[56] = 0;
out_5465143194872640210[57] = 0;
out_5465143194872640210[58] = 0;
out_5465143194872640210[59] = 0;
out_5465143194872640210[60] = 1.0;
out_5465143194872640210[61] = 0;
out_5465143194872640210[62] = 0;
out_5465143194872640210[63] = 0;
out_5465143194872640210[64] = 0;
out_5465143194872640210[65] = 0;
out_5465143194872640210[66] = 0;
out_5465143194872640210[67] = 0;
out_5465143194872640210[68] = 0;
out_5465143194872640210[69] = 0;
out_5465143194872640210[70] = 0;
out_5465143194872640210[71] = 0;
out_5465143194872640210[72] = 1.0;
out_5465143194872640210[73] = 0;
out_5465143194872640210[74] = 0;
out_5465143194872640210[75] = 0;
out_5465143194872640210[76] = 0;
out_5465143194872640210[77] = 0;
out_5465143194872640210[78] = 0;
out_5465143194872640210[79] = 0;
out_5465143194872640210[80] = 0;
out_5465143194872640210[81] = 0;
out_5465143194872640210[82] = 0;
out_5465143194872640210[83] = 0;
out_5465143194872640210[84] = 1.0;
out_5465143194872640210[85] = 0;
out_5465143194872640210[86] = 0;
out_5465143194872640210[87] = 0;
out_5465143194872640210[88] = 0;
out_5465143194872640210[89] = 0;
out_5465143194872640210[90] = 0;
out_5465143194872640210[91] = 0;
out_5465143194872640210[92] = 0;
out_5465143194872640210[93] = 0;
out_5465143194872640210[94] = 0;
out_5465143194872640210[95] = 0;
out_5465143194872640210[96] = 1.0;
out_5465143194872640210[97] = 0;
out_5465143194872640210[98] = 0;
out_5465143194872640210[99] = 0;
out_5465143194872640210[100] = 0;
out_5465143194872640210[101] = 0;
out_5465143194872640210[102] = 0;
out_5465143194872640210[103] = 0;
out_5465143194872640210[104] = 0;
out_5465143194872640210[105] = 0;
out_5465143194872640210[106] = 0;
out_5465143194872640210[107] = 0;
out_5465143194872640210[108] = 1.0;
out_5465143194872640210[109] = 0;
out_5465143194872640210[110] = 0;
out_5465143194872640210[111] = 0;
out_5465143194872640210[112] = 0;
out_5465143194872640210[113] = 0;
out_5465143194872640210[114] = 0;
out_5465143194872640210[115] = 0;
out_5465143194872640210[116] = 0;
out_5465143194872640210[117] = 0;
out_5465143194872640210[118] = 0;
out_5465143194872640210[119] = 0;
out_5465143194872640210[120] = 1.0;
}
void f_fun(double *state, double dt, double *out_4766656596405504851) {
out_4766656596405504851[0] = dt*state[3] + state[0];
out_4766656596405504851[1] = dt*state[4] + state[1];
out_4766656596405504851[2] = dt*state[5] + state[2];
out_4766656596405504851[3] = state[3];
out_4766656596405504851[4] = state[4];
out_4766656596405504851[5] = state[5];
out_4766656596405504851[6] = dt*state[7] + state[6];
out_4766656596405504851[7] = dt*state[8] + state[7];
out_4766656596405504851[8] = state[8];
out_4766656596405504851[9] = state[9];
out_4766656596405504851[10] = state[10];
void f_fun(double *state, double dt, double *out_6477589900664112980) {
out_6477589900664112980[0] = dt*state[3] + state[0];
out_6477589900664112980[1] = dt*state[4] + state[1];
out_6477589900664112980[2] = dt*state[5] + state[2];
out_6477589900664112980[3] = state[3];
out_6477589900664112980[4] = state[4];
out_6477589900664112980[5] = state[5];
out_6477589900664112980[6] = dt*state[7] + state[6];
out_6477589900664112980[7] = dt*state[8] + state[7];
out_6477589900664112980[8] = state[8];
out_6477589900664112980[9] = state[9];
out_6477589900664112980[10] = state[10];
}
void F_fun(double *state, double dt, double *out_4029299056587768608) {
out_4029299056587768608[0] = 1;
out_4029299056587768608[1] = 0;
out_4029299056587768608[2] = 0;
out_4029299056587768608[3] = dt;
out_4029299056587768608[4] = 0;
out_4029299056587768608[5] = 0;
out_4029299056587768608[6] = 0;
out_4029299056587768608[7] = 0;
out_4029299056587768608[8] = 0;
out_4029299056587768608[9] = 0;
out_4029299056587768608[10] = 0;
out_4029299056587768608[11] = 0;
out_4029299056587768608[12] = 1;
out_4029299056587768608[13] = 0;
out_4029299056587768608[14] = 0;
out_4029299056587768608[15] = dt;
out_4029299056587768608[16] = 0;
out_4029299056587768608[17] = 0;
out_4029299056587768608[18] = 0;
out_4029299056587768608[19] = 0;
out_4029299056587768608[20] = 0;
out_4029299056587768608[21] = 0;
out_4029299056587768608[22] = 0;
out_4029299056587768608[23] = 0;
out_4029299056587768608[24] = 1;
out_4029299056587768608[25] = 0;
out_4029299056587768608[26] = 0;
out_4029299056587768608[27] = dt;
out_4029299056587768608[28] = 0;
out_4029299056587768608[29] = 0;
out_4029299056587768608[30] = 0;
out_4029299056587768608[31] = 0;
out_4029299056587768608[32] = 0;
out_4029299056587768608[33] = 0;
out_4029299056587768608[34] = 0;
out_4029299056587768608[35] = 0;
out_4029299056587768608[36] = 1;
out_4029299056587768608[37] = 0;
out_4029299056587768608[38] = 0;
out_4029299056587768608[39] = 0;
out_4029299056587768608[40] = 0;
out_4029299056587768608[41] = 0;
out_4029299056587768608[42] = 0;
out_4029299056587768608[43] = 0;
out_4029299056587768608[44] = 0;
out_4029299056587768608[45] = 0;
out_4029299056587768608[46] = 0;
out_4029299056587768608[47] = 0;
out_4029299056587768608[48] = 1;
out_4029299056587768608[49] = 0;
out_4029299056587768608[50] = 0;
out_4029299056587768608[51] = 0;
out_4029299056587768608[52] = 0;
out_4029299056587768608[53] = 0;
out_4029299056587768608[54] = 0;
out_4029299056587768608[55] = 0;
out_4029299056587768608[56] = 0;
out_4029299056587768608[57] = 0;
out_4029299056587768608[58] = 0;
out_4029299056587768608[59] = 0;
out_4029299056587768608[60] = 1;
out_4029299056587768608[61] = 0;
out_4029299056587768608[62] = 0;
out_4029299056587768608[63] = 0;
out_4029299056587768608[64] = 0;
out_4029299056587768608[65] = 0;
out_4029299056587768608[66] = 0;
out_4029299056587768608[67] = 0;
out_4029299056587768608[68] = 0;
out_4029299056587768608[69] = 0;
out_4029299056587768608[70] = 0;
out_4029299056587768608[71] = 0;
out_4029299056587768608[72] = 1;
out_4029299056587768608[73] = dt;
out_4029299056587768608[74] = 0;
out_4029299056587768608[75] = 0;
out_4029299056587768608[76] = 0;
out_4029299056587768608[77] = 0;
out_4029299056587768608[78] = 0;
out_4029299056587768608[79] = 0;
out_4029299056587768608[80] = 0;
out_4029299056587768608[81] = 0;
out_4029299056587768608[82] = 0;
out_4029299056587768608[83] = 0;
out_4029299056587768608[84] = 1;
out_4029299056587768608[85] = dt;
out_4029299056587768608[86] = 0;
out_4029299056587768608[87] = 0;
out_4029299056587768608[88] = 0;
out_4029299056587768608[89] = 0;
out_4029299056587768608[90] = 0;
out_4029299056587768608[91] = 0;
out_4029299056587768608[92] = 0;
out_4029299056587768608[93] = 0;
out_4029299056587768608[94] = 0;
out_4029299056587768608[95] = 0;
out_4029299056587768608[96] = 1;
out_4029299056587768608[97] = 0;
out_4029299056587768608[98] = 0;
out_4029299056587768608[99] = 0;
out_4029299056587768608[100] = 0;
out_4029299056587768608[101] = 0;
out_4029299056587768608[102] = 0;
out_4029299056587768608[103] = 0;
out_4029299056587768608[104] = 0;
out_4029299056587768608[105] = 0;
out_4029299056587768608[106] = 0;
out_4029299056587768608[107] = 0;
out_4029299056587768608[108] = 1;
out_4029299056587768608[109] = 0;
out_4029299056587768608[110] = 0;
out_4029299056587768608[111] = 0;
out_4029299056587768608[112] = 0;
out_4029299056587768608[113] = 0;
out_4029299056587768608[114] = 0;
out_4029299056587768608[115] = 0;
out_4029299056587768608[116] = 0;
out_4029299056587768608[117] = 0;
out_4029299056587768608[118] = 0;
out_4029299056587768608[119] = 0;
out_4029299056587768608[120] = 1;
void F_fun(double *state, double dt, double *out_159332577828599777) {
out_159332577828599777[0] = 1;
out_159332577828599777[1] = 0;
out_159332577828599777[2] = 0;
out_159332577828599777[3] = dt;
out_159332577828599777[4] = 0;
out_159332577828599777[5] = 0;
out_159332577828599777[6] = 0;
out_159332577828599777[7] = 0;
out_159332577828599777[8] = 0;
out_159332577828599777[9] = 0;
out_159332577828599777[10] = 0;
out_159332577828599777[11] = 0;
out_159332577828599777[12] = 1;
out_159332577828599777[13] = 0;
out_159332577828599777[14] = 0;
out_159332577828599777[15] = dt;
out_159332577828599777[16] = 0;
out_159332577828599777[17] = 0;
out_159332577828599777[18] = 0;
out_159332577828599777[19] = 0;
out_159332577828599777[20] = 0;
out_159332577828599777[21] = 0;
out_159332577828599777[22] = 0;
out_159332577828599777[23] = 0;
out_159332577828599777[24] = 1;
out_159332577828599777[25] = 0;
out_159332577828599777[26] = 0;
out_159332577828599777[27] = dt;
out_159332577828599777[28] = 0;
out_159332577828599777[29] = 0;
out_159332577828599777[30] = 0;
out_159332577828599777[31] = 0;
out_159332577828599777[32] = 0;
out_159332577828599777[33] = 0;
out_159332577828599777[34] = 0;
out_159332577828599777[35] = 0;
out_159332577828599777[36] = 1;
out_159332577828599777[37] = 0;
out_159332577828599777[38] = 0;
out_159332577828599777[39] = 0;
out_159332577828599777[40] = 0;
out_159332577828599777[41] = 0;
out_159332577828599777[42] = 0;
out_159332577828599777[43] = 0;
out_159332577828599777[44] = 0;
out_159332577828599777[45] = 0;
out_159332577828599777[46] = 0;
out_159332577828599777[47] = 0;
out_159332577828599777[48] = 1;
out_159332577828599777[49] = 0;
out_159332577828599777[50] = 0;
out_159332577828599777[51] = 0;
out_159332577828599777[52] = 0;
out_159332577828599777[53] = 0;
out_159332577828599777[54] = 0;
out_159332577828599777[55] = 0;
out_159332577828599777[56] = 0;
out_159332577828599777[57] = 0;
out_159332577828599777[58] = 0;
out_159332577828599777[59] = 0;
out_159332577828599777[60] = 1;
out_159332577828599777[61] = 0;
out_159332577828599777[62] = 0;
out_159332577828599777[63] = 0;
out_159332577828599777[64] = 0;
out_159332577828599777[65] = 0;
out_159332577828599777[66] = 0;
out_159332577828599777[67] = 0;
out_159332577828599777[68] = 0;
out_159332577828599777[69] = 0;
out_159332577828599777[70] = 0;
out_159332577828599777[71] = 0;
out_159332577828599777[72] = 1;
out_159332577828599777[73] = dt;
out_159332577828599777[74] = 0;
out_159332577828599777[75] = 0;
out_159332577828599777[76] = 0;
out_159332577828599777[77] = 0;
out_159332577828599777[78] = 0;
out_159332577828599777[79] = 0;
out_159332577828599777[80] = 0;
out_159332577828599777[81] = 0;
out_159332577828599777[82] = 0;
out_159332577828599777[83] = 0;
out_159332577828599777[84] = 1;
out_159332577828599777[85] = dt;
out_159332577828599777[86] = 0;
out_159332577828599777[87] = 0;
out_159332577828599777[88] = 0;
out_159332577828599777[89] = 0;
out_159332577828599777[90] = 0;
out_159332577828599777[91] = 0;
out_159332577828599777[92] = 0;
out_159332577828599777[93] = 0;
out_159332577828599777[94] = 0;
out_159332577828599777[95] = 0;
out_159332577828599777[96] = 1;
out_159332577828599777[97] = 0;
out_159332577828599777[98] = 0;
out_159332577828599777[99] = 0;
out_159332577828599777[100] = 0;
out_159332577828599777[101] = 0;
out_159332577828599777[102] = 0;
out_159332577828599777[103] = 0;
out_159332577828599777[104] = 0;
out_159332577828599777[105] = 0;
out_159332577828599777[106] = 0;
out_159332577828599777[107] = 0;
out_159332577828599777[108] = 1;
out_159332577828599777[109] = 0;
out_159332577828599777[110] = 0;
out_159332577828599777[111] = 0;
out_159332577828599777[112] = 0;
out_159332577828599777[113] = 0;
out_159332577828599777[114] = 0;
out_159332577828599777[115] = 0;
out_159332577828599777[116] = 0;
out_159332577828599777[117] = 0;
out_159332577828599777[118] = 0;
out_159332577828599777[119] = 0;
out_159332577828599777[120] = 1;
}
void h_6(double *state, double *sat_pos, double *out_1067178731754715050) {
out_1067178731754715050[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_822185378282268145) {
out_822185378282268145[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_7962220724729872721) {
out_7962220724729872721[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_7962220724729872721[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_7962220724729872721[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_7962220724729872721[3] = 0;
out_7962220724729872721[4] = 0;
out_7962220724729872721[5] = 0;
out_7962220724729872721[6] = 1;
out_7962220724729872721[7] = 0;
out_7962220724729872721[8] = 0;
out_7962220724729872721[9] = 0;
out_7962220724729872721[10] = 0;
void H_6(double *state, double *sat_pos, double *out_2954217689973778176) {
out_2954217689973778176[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_2954217689973778176[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_2954217689973778176[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_2954217689973778176[3] = 0;
out_2954217689973778176[4] = 0;
out_2954217689973778176[5] = 0;
out_2954217689973778176[6] = 1;
out_2954217689973778176[7] = 0;
out_2954217689973778176[8] = 0;
out_2954217689973778176[9] = 0;
out_2954217689973778176[10] = 0;
}
void h_20(double *state, double *sat_pos, double *out_7444431052127272697) {
out_7444431052127272697[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_3775340380998125465) {
out_3775340380998125465[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_799492558915393744) {
out_799492558915393744[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_799492558915393744[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_799492558915393744[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_799492558915393744[3] = 0;
out_799492558915393744[4] = 0;
out_799492558915393744[5] = 0;
out_799492558915393744[6] = 1;
out_799492558915393744[7] = 0;
out_799492558915393744[8] = 0;
out_799492558915393744[9] = 1;
out_799492558915393744[10] = sat_pos[3];
void H_20(double *state, double *sat_pos, double *out_4124080333257632986) {
out_4124080333257632986[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_4124080333257632986[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_4124080333257632986[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_4124080333257632986[3] = 0;
out_4124080333257632986[4] = 0;
out_4124080333257632986[5] = 0;
out_4124080333257632986[6] = 1;
out_4124080333257632986[7] = 0;
out_4124080333257632986[8] = 0;
out_4124080333257632986[9] = 1;
out_4124080333257632986[10] = sat_pos[3];
}
void h_7(double *state, double *sat_pos_vel, double *out_7430002568771021643) {
out_7430002568771021643[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_750344228235882460) {
out_750344228235882460[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_7060224876276749950) {
out_7060224876276749950[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_7060224876276749950[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_7060224876276749950[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_7060224876276749950[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_7060224876276749950[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_7060224876276749950[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_7060224876276749950[6] = 0;
out_7060224876276749950[7] = 1;
out_7060224876276749950[8] = 0;
out_7060224876276749950[9] = 0;
out_7060224876276749950[10] = 0;
void H_7(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
out_1618262330099536806[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_1618262330099536806[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_1618262330099536806[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_1618262330099536806[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_1618262330099536806[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_1618262330099536806[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_1618262330099536806[6] = 0;
out_1618262330099536806[7] = 1;
out_1618262330099536806[8] = 0;
out_1618262330099536806[9] = 0;
out_1618262330099536806[10] = 0;
}
void h_21(double *state, double *sat_pos_vel, double *out_7430002568771021643) {
out_7430002568771021643[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_750344228235882460) {
out_750344228235882460[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_7060224876276749950) {
out_7060224876276749950[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_7060224876276749950[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_7060224876276749950[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_7060224876276749950[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_7060224876276749950[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_7060224876276749950[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_7060224876276749950[6] = 0;
out_7060224876276749950[7] = 1;
out_7060224876276749950[8] = 0;
out_7060224876276749950[9] = 0;
out_7060224876276749950[10] = 0;
void H_21(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
out_1618262330099536806[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_1618262330099536806[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_1618262330099536806[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_1618262330099536806[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_1618262330099536806[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_1618262330099536806[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_1618262330099536806[6] = 0;
out_1618262330099536806[7] = 1;
out_1618262330099536806[8] = 0;
out_1618262330099536806[9] = 0;
out_1618262330099536806[10] = 0;
}
#include <eigen3/Eigen/Dense>
#include <iostream>
@@ -506,44 +506,44 @@ void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_21, H_21, NULL, in_z, in_R, in_ea, MAHA_THRESH_21);
}
void gnss_err_fun(double *nom_x, double *delta_x, double *out_654910612427940843) {
err_fun(nom_x, delta_x, out_654910612427940843);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_8218999594846759964) {
err_fun(nom_x, delta_x, out_8218999594846759964);
}
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6637724801134549563) {
inv_err_fun(nom_x, true_x, out_6637724801134549563);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6004490369099356165) {
inv_err_fun(nom_x, true_x, out_6004490369099356165);
}
void gnss_H_mod_fun(double *state, double *out_805386010827368873) {
H_mod_fun(state, out_805386010827368873);
void gnss_H_mod_fun(double *state, double *out_5465143194872640210) {
H_mod_fun(state, out_5465143194872640210);
}
void gnss_f_fun(double *state, double dt, double *out_4766656596405504851) {
f_fun(state, dt, out_4766656596405504851);
void gnss_f_fun(double *state, double dt, double *out_6477589900664112980) {
f_fun(state, dt, out_6477589900664112980);
}
void gnss_F_fun(double *state, double dt, double *out_4029299056587768608) {
F_fun(state, dt, out_4029299056587768608);
void gnss_F_fun(double *state, double dt, double *out_159332577828599777) {
F_fun(state, dt, out_159332577828599777);
}
void gnss_h_6(double *state, double *sat_pos, double *out_1067178731754715050) {
h_6(state, sat_pos, out_1067178731754715050);
void gnss_h_6(double *state, double *sat_pos, double *out_822185378282268145) {
h_6(state, sat_pos, out_822185378282268145);
}
void gnss_H_6(double *state, double *sat_pos, double *out_7962220724729872721) {
H_6(state, sat_pos, out_7962220724729872721);
void gnss_H_6(double *state, double *sat_pos, double *out_2954217689973778176) {
H_6(state, sat_pos, out_2954217689973778176);
}
void gnss_h_20(double *state, double *sat_pos, double *out_7444431052127272697) {
h_20(state, sat_pos, out_7444431052127272697);
void gnss_h_20(double *state, double *sat_pos, double *out_3775340380998125465) {
h_20(state, sat_pos, out_3775340380998125465);
}
void gnss_H_20(double *state, double *sat_pos, double *out_799492558915393744) {
H_20(state, sat_pos, out_799492558915393744);
void gnss_H_20(double *state, double *sat_pos, double *out_4124080333257632986) {
H_20(state, sat_pos, out_4124080333257632986);
}
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7430002568771021643) {
h_7(state, sat_pos_vel, out_7430002568771021643);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_750344228235882460) {
h_7(state, sat_pos_vel, out_750344228235882460);
}
void gnss_H_7(double *state, double *sat_pos_vel, double *out_7060224876276749950) {
H_7(state, sat_pos_vel, out_7060224876276749950);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
H_7(state, sat_pos_vel, out_1618262330099536806);
}
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7430002568771021643) {
h_21(state, sat_pos_vel, out_7430002568771021643);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_750344228235882460) {
h_21(state, sat_pos_vel, out_750344228235882460);
}
void gnss_H_21(double *state, double *sat_pos_vel, double *out_7060224876276749950) {
H_21(state, sat_pos_vel, out_7060224876276749950);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1618262330099536806) {
H_21(state, sat_pos_vel, out_1618262330099536806);
}
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
selfdrive/locationd/models/generated/gnss.h
+13 -13
View File
@@ -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_654910612427940843);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6637724801134549563);
void gnss_H_mod_fun(double *state, double *out_805386010827368873);
void gnss_f_fun(double *state, double dt, double *out_4766656596405504851);
void gnss_F_fun(double *state, double dt, double *out_4029299056587768608);
void gnss_h_6(double *state, double *sat_pos, double *out_1067178731754715050);
void gnss_H_6(double *state, double *sat_pos, double *out_7962220724729872721);
void gnss_h_20(double *state, double *sat_pos, double *out_7444431052127272697);
void gnss_H_20(double *state, double *sat_pos, double *out_799492558915393744);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7430002568771021643);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_7060224876276749950);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7430002568771021643);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_7060224876276749950);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_8218999594846759964);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6004490369099356165);
void gnss_H_mod_fun(double *state, double *out_5465143194872640210);
void gnss_f_fun(double *state, double dt, double *out_6477589900664112980);
void gnss_F_fun(double *state, double dt, double *out_159332577828599777);
void gnss_h_6(double *state, double *sat_pos, double *out_822185378282268145);
void gnss_H_6(double *state, double *sat_pos, double *out_2954217689973778176);
void gnss_h_20(double *state, double *sat_pos, double *out_3775340380998125465);
void gnss_H_20(double *state, double *sat_pos, double *out_4124080333257632986);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_750344228235882460);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1618262330099536806);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_750344228235882460);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1618262330099536806);
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/locationd/models/generated/libkf.so
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/live.cpp
+1702 -1702
View File
File diff suppressed because it is too large Load Diff
selfdrive/locationd/models/generated/live.h
+24 -24
View File
@@ -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_2989134609448912360);
void live_err_fun(double *nom_x, double *delta_x, double *out_4134642150611912562);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_102578031842580524);
void live_H_mod_fun(double *state, double *out_1333467034631963441);
void live_f_fun(double *state, double dt, double *out_1638653065682139689);
void live_F_fun(double *state, double dt, double *out_3627592268953963269);
void live_h_4(double *state, double *unused, double *out_8187072558274677226);
void live_H_4(double *state, double *unused, double *out_5073553165915915446);
void live_h_9(double *state, double *unused, double *out_7751183598308146162);
void live_H_9(double *state, double *unused, double *out_6085971972529188700);
void live_h_10(double *state, double *unused, double *out_7475415363803884616);
void live_H_10(double *state, double *unused, double *out_7599748917935120017);
void live_h_12(double *state, double *unused, double *out_4560316742219209161);
void live_H_12(double *state, double *unused, double *out_8353734499761674375);
void live_h_35(double *state, double *unused, double *out_395256144382285655);
void live_H_35(double *state, double *unused, double *out_5608171467436660666);
void live_h_32(double *state, double *unused, double *out_2629447041285500641);
void live_H_32(double *state, double *unused, double *out_5477006682495920197);
void live_h_13(double *state, double *unused, double *out_4411752618062649722);
void live_H_13(double *state, double *unused, double *out_6527861414729128748);
void live_h_14(double *state, double *unused, double *out_7751183598308146162);
void live_H_14(double *state, double *unused, double *out_6085971972529188700);
void live_h_33(double *state, double *unused, double *out_6706316243285295529);
void live_H_33(double *state, double *unused, double *out_2457614462797803062);
void live_H(double *in_vec, double *out_7959907782308194627);
void live_err_fun(double *nom_x, double *delta_x, double *out_6929774310989775988);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_6193789776824585571);
void live_H_mod_fun(double *state, double *out_8547509152961498922);
void live_f_fun(double *state, double dt, double *out_8100393315288221642);
void live_F_fun(double *state, double dt, double *out_7769089323050102648);
void live_h_4(double *state, double *unused, double *out_6579737165675804813);
void live_H_4(double *state, double *unused, double *out_8985066658053328462);
void live_h_9(double *state, double *unused, double *out_6592276556226636800);
void live_H_9(double *state, double *unused, double *out_6096205105773249120);
void live_h_10(double *state, double *unused, double *out_3043512482999151809);
void live_H_10(double *state, double *unused, double *out_7192958685017806166);
void live_h_12(double *state, double *unused, double *out_6151089362108428494);
void live_H_12(double *state, double *unused, double *out_1317938344370877970);
void live_h_35(double *state, double *unused, double *out_1134950865722680502);
void live_H_35(double *state, double *unused, double *out_1427624687954135739);
void live_h_32(double *state, double *unused, double *out_4721207292266360875);
void live_H_32(double *state, double *unused, double *out_2913380087778971149);
void live_h_13(double *state, double *unused, double *out_364134050257467252);
void live_H_13(double *state, double *unused, double *out_5486592313647626777);
void live_h_14(double *state, double *unused, double *out_6592276556226636800);
void live_H_14(double *state, double *unused, double *out_6096205105773249120);
void live_h_33(double *state, double *unused, double *out_3737322617129334857);
void live_H_33(double *state, double *unused, double *out_4578181692592993343);
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/logcatd/logcatd
BIN
View File
Binary file not shown.
selfdrive/loggerd/bootlog
BIN
View File
Binary file not shown.
selfdrive/loggerd/loggerd
BIN
View File
Binary file not shown.
selfdrive/loggerd/uploader.py
+4
View File
@@ -70,6 +70,7 @@ class Uploader():
self.immediate_folders = ["crash/", "boot/"]
self.immediate_priority = {"qlog.bz2": 0, "qcamera.ts": 1}
# self.dp_upload_ignored = Params().get_bool('dp_upload_ignored')
def get_upload_sort(self, name):
if name in self.immediate_priority:
@@ -195,6 +196,9 @@ class Uploader():
self.last_time = time.monotonic() - start_time
self.last_speed = (sz / 1e6) / self.last_time
success = True
# if not self.dp_upload_ignored and stat.status_code == 412:
# self.dp_upload_ignored = True
# Params().put_bool('dp_upload_ignored', True)
cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type)
else:
success = False
selfdrive/manager/local_osm_install.py
+21 -22
View File
@@ -1,27 +1,26 @@
#!/usr/bin/env python3
import os
import subprocess
import sys
# NOTE: Do NOT import anything here that needs be built (e.g. params)
from common.basedir import BASEDIR
from common.params import Params
from common.spinner import Spinner
from selfdrive.mapd.lib.helpers import is_local_osm_installed, timestamp_local_osm_db, is_osm_db_up_to_date, OSM_LOCAL_PATH, OSM_VERSION
from selfdrive.manager.custom_dep import wait_for_internet_connection
import time
# MAX_BUILD_PROGRESS = 100
# INSTALL_OSM_STEPS = 1021
# INSTALL_DB_STEPS = 58
def install_local_osm(spinner): #, steps=0, total_steps=INSTALL_OSM_STEPS):
_install(spinner, "./install_osm.sh", "Installing OSM Server") #, steps, total_steps)
def install_local_osm(_spinner):
_install(_spinner, "./install_osm.sh", "Installing OSM Server")
def install_osm_db(spinner): #, steps=0, total_steps=INSTALL_DB_STEPS):
_install(spinner, "./install_osm_db.sh", "Installing OSM DB - " + Params().get("OsmLocationName", encoding="utf-8")) #, steps, total_steps)
def _install(spinner, script, title): #, steps, total_steps):
spinner.update(title)
def install_osm_db(_spinner):
_install(_spinner, "./install_osm_db.sh", "Installing OSM DB - " + Params().get("OsmLocationName", encoding="utf-8"))
def _install(_spinner, script, title):
_spinner.update(title)
process = subprocess.Popen(['sh', script], cwd=os.path.join(BASEDIR, 'installer/custom/'),
stdout=subprocess.PIPE)
# Read progress from install script and update spinner
@@ -30,31 +29,31 @@ def _install(spinner, script, title): #, steps, total_steps):
output = process.stdout.readline()
if process.poll() is not None:
break
spinner.update(title + (".".replace(".", "." * (frame % 5), 1)))
_spinner.update(title + (".".replace(".", "." * (frame % 5), 1)))
frame += 1
print(output.decode('utf8', 'replace'))
time.sleep(0.1)
# if output:
# steps += 1
# spinner.update(title + ": " + str(int(min(MAX_BUILD_PROGRESS * min(1., steps / total_steps), 100.))) + "%")
# print(output.decode('utf8', 'replace'))
if __name__ == "__main__":
update_osm_db_check = Params().get_bool("OsmDbUpdatesCheck")
from selfdrive.mapd.lib.helpers import is_local_osm_installed, timestamp_local_osm_db, is_osm_db_up_to_date, OSM_LOCAL_PATH
sys.path.append(os.path.join(BASEDIR, "third_party/mapd"))
params = Params()
update_osm_db_check = params.get_bool("OsmDbUpdatesCheck")
if not (os.path.exists(f"{OSM_LOCAL_PATH}/db") or
os.path.exists(f"{OSM_LOCAL_PATH}/{OSM_VERSION}")) or update_osm_db_check:
os.path.exists(f"{OSM_LOCAL_PATH}/v0.7.57")) or update_osm_db_check:
spinner = Spinner()
spinner.update("Waiting for internet connection...")
if wait_for_internet_connection(return_on_failure=True):
is_osm_installed = is_local_osm_installed()
is_osm_installed = is_local_osm_installed(params)
is_db_updated = is_osm_db_up_to_date()
print(f'Local OSM Installer:\nOSM currently installed: {is_osm_installed}\nDB up to date: {is_db_updated}')
if not is_osm_installed:
install_local_osm(spinner) #, total_steps=INSTALL_OSM_STEPS)
if not is_db_updated or update_osm_db_check:
install_osm_db(spinner) #, total_steps=INSTALL_DB_STEPS)
install_local_osm(spinner)
if not is_db_updated:
install_osm_db(spinner)
timestamp_local_osm_db()
spinner.close()
Params().put_bool("OsmDbUpdatesCheck", False)
params.put_bool("OsmDbUpdatesCheck", False)
selfdrive/manager/manager.py
+7 -3
View File
@@ -13,7 +13,7 @@ from common.basedir import BASEDIR
from common.params import Params, ParamKeyType
from common.text_window import TextWindow
from selfdrive.boardd.set_time import set_time
from system.hardware import HARDWARE, PC
from system.hardware import HARDWARE, PC, TICI
from selfdrive.manager.helpers import unblock_stdout
from selfdrive.manager.process import ensure_running
from selfdrive.manager.process_config import managed_processes
@@ -32,7 +32,8 @@ def manager_init() -> None:
set_time(cloudlog)
# save boot log
# subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "system/loggerd"))
# if not Params().get_bool('dp_jetson'):
# subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "system/loggerd"))
params = Params()
params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START)
@@ -134,6 +135,9 @@ def manager_thread() -> None:
ignore: List[str] = []
# dp
if TICI:
params.put_bool('dp_dm', True)
params.put_bool('dp_jetson', False)
dp_nav = params.get_bool('dp_nav')
dp_otisserv = dp_nav and params.get_bool('dp_otisserv')
dp_jetson = params.get_bool('dp_jetson')
@@ -178,7 +182,7 @@ def manager_thread() -> None:
for param in ("DoUninstall", "DoShutdown", "DoReboot"):
if params.get_bool(param):
shutdown = True
params.put("LastManagerExitReason", param)
params.put("LastManagerExitReason", f"{param} {datetime.datetime.now()}")
cloudlog.warning(f"Shutting down manager - {param} set")
if shutdown:
selfdrive/manager/process_config.py
+8 -2
View File
@@ -19,11 +19,17 @@ def logging(started, params, CP: car.CarParams) -> bool:
run = (not CP.notCar) or not params.get_bool("DisableLogging")
return started and run
def ublox_available() -> bool:
return os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps')
def ublox(started, params, CP: car.CarParams) -> bool:
use_ublox = True #os.path.exists('/dev/ttyHS0') and not os.path.exists('/persist/comma/use-quectel-gps')
use_ublox = True # ublox_available()
params.put_bool("UbloxAvailable", use_ublox)
return started and use_ublox
def qcomgps(started, params, CP: car.CarParams) -> bool:
return started and not ublox_available()
procs = [
# due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption
NativeProcess("camerad", "system/camerad", ["./camerad"], unkillable=True, callback=driverview),
@@ -52,7 +58,7 @@ procs = [
# PythonProcess("deleter", "system.loggerd.deleter", offroad=True),
# PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), callback=driverview),
# PythonProcess("laikad", "selfdrive.locationd.laikad"),
# PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", enabled=TICI),
# PythonProcess("rawgpsd", "system.sensord.rawgps.rawgpsd", enabled=TICI, onroad=False, callback=qcomgps),
PythonProcess("navd", "selfdrive.navd.navd"),
PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
selfdrive/mapd/lib/WayRelation.py
+2
View File
@@ -146,6 +146,8 @@ def conditional_speed_limit_for_osm_tag_limit_string(limit_string):
def speed_limit_value_for_highway_type(areas, tags):
max_speed = None
try:
geocode_country = ''
geocode_region = ''
for area in areas:
if area.tags.get('admin_level', '') == "2":
if area.tags.get('ISO3166-1:alpha2', '') != '':
selfdrive/mapd/lib/helpers.py
+20 -23
View File
@@ -1,37 +1,37 @@
import overpy
import subprocess
import requests
import email.utils as eut
import time
from common.params import Params
from system.hardware import TICI
from selfdrive.hardware import TICI
from system.version import get_version
from urllib.request import Request, urlopen
OSM_LOCAL_PATH = "/data/media/0/osm"
OSM_DB_STAMP_FILE = OSM_LOCAL_PATH + "/db_stamp"
OSM_VERSION = "0.7.57" if TICI else "0.7.56"
OSM_QUERY = [f"{OSM_LOCAL_PATH}/v{OSM_VERSION}/bin/osm3s_query", f"--db-dir={OSM_LOCAL_PATH}/db"]
OSM_DB_STAMP_REMOTE = "https://sunnypilot-osm.s3.us-east-2.amazonaws.com/osm-db/timestamps"
def get_current_s3_osm_db_timestamp():
local_osm_db_name = Params().get("OsmLocationName")
if local_osm_db_name is None:
return None
local_osm_db_name = local_osm_db_name.decode("utf-8")
r = requests.head(f"https://mkumard.synology.me/osm/{local_osm_db_name}.txt")
if r.status_code != 200:
print(f'Failed to fetch HEAD for S3 OSM db.\n\n{r.status_code}')
return None
timestamp_string = r.headers.get('Last-Modified', None)
if timestamp_string is None:
print(f'HEAD for S3 OSM db contained no "Last-Modified" value.\n\n{r.headers}')
return None
try:
local_osm_db_name = Params().get("OsmLocationName", encoding="utf8")
req = Request(url=f"{OSM_DB_STAMP_REMOTE}/{local_osm_db_name}.txt", headers={"User-Agent": f"dragonpilot-{get_version()}"})
r = urlopen(req)
if r.status != 200:
print(f'Failed to fetch timestamp for S3 OSM db.\n\n{r.status}')
return None
timestamp_string = r.read().decode("utf-8").strip()
if timestamp_string is None:
print('Timestamp file for S3 OSM db contained no value.')
return None
parsed_date = eut.parsedate(timestamp_string)
return time.mktime(parsed_date)
except Exception as e:
print(f'Could not parse last modified timestamp for S3 local osm db.\n\n{e}')
print(f'Could not parse timestamp for S3 local osm db.\n\n{e}')
return None
@@ -70,12 +70,11 @@ def timestamp_local_osm_db():
persist_s3_osm_db_timestamp(current_s3_osm_ts)
def is_local_osm_installed():
def is_local_osm_installed(params=Params()):
api = overpy.Overpass()
waypoint = Params().get("OsmWayTest")
waypoint = params.get("OsmWayTest", encoding="utf8")
if waypoint is None:
return False
waypoint = waypoint.decode("utf-8")
q = f"""
way({waypoint});
(._;>;);
@@ -83,9 +82,7 @@ def is_local_osm_installed():
"""
try:
cmd = OSM_QUERY
cmd.append(f"--request={q}")
completion = subprocess.run(cmd, check=True, capture_output=True)
completion = subprocess.run(OSM_QUERY + [f"--request={q}"], check=True, capture_output=True)
print(f'OSM local query returned with exit code: {completion.returncode}')
if completion.returncode != 0:
selfdrive/mapd/sp_osm_db.json
-9
View File
@@ -1,9 +0,0 @@
{
"N/A": "",
"US - West": "https://thecorgirosie-my.sharepoint.com/:u:/p/jason_wen/EYWdXcNpOhJCuOLymUHSwH4BwdRddfiYEbZAK1ltd-hZJA?e=TW5ia4&download=1",
"US - South": "https://thecorgirosie-my.sharepoint.com/:u:/p/jason_wen/EWNHW-27AzRIr8KRd3EGMkABVEoXf8KLz3uRvvEgfP7xAw?e=4Cbkcu&download=1",
"TW": "https://thecorgirosie-my.sharepoint.com/:u:/p/jason_wen/EYdsed4UDtNFtR7z6_3nffkBqS40qoO9kGZfKGsfz51g5w?e=Werz8B&download=1",
"US - Florida": "https://thecorgirosie-my.sharepoint.com/:u:/p/jason_wen/EfSUnV9PtLFCjDiTrvMZOPIBVeg1SYjMQOCq-FsoUrv5vg?e=yaDpL2&download=1",
"US - Northeast": "https://thecorgirosie-my.sharepoint.com/:u:/p/jason_wen/Ecx2vyduMtBLiYrBCJ6eu-YBYvmztE25F7s00FvPeUemzw?e=060Zlj&download=1",
"ZA": "https://thecorgirosie-my.sharepoint.com/:u:/p/jason_wen/ESBBZE76YPdCv19nN-F_zJ8BfnOseAdr692rtLsDT784kA?e=Aq2kiJ&download=1"
}
selfdrive/modeld/_dmonitoringmodeld
BIN
View File
Binary file not shown.
selfdrive/modeld/_modeld
BIN
View File
Binary file not shown.
selfdrive/modeld/models/supercombo.thneed
BIN
View File
Binary file not shown.
selfdrive/proclogd/proclogd
BIN
View File
Binary file not shown.
selfdrive/ui/_ui
BIN
View File
Binary file not shown.
selfdrive/ui/_ui_nonav
BIN
View File
Binary file not shown.
selfdrive/ui/qt/spinner
BIN
View File
Binary file not shown.
selfdrive/ui/qt/text
BIN
View File
Binary file not shown.
selfdrive/ui/soundd/_soundd
BIN
View File
Binary file not shown.
selfdrive/ui/translations/main_pt-BR.ts
+6 -5
View File
@@ -1431,16 +1431,17 @@ trabalho definido</translation>
</message>
<message>
<source>Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device.</source>
<translation type="unfinished"></translation>
<translation>Não é possível montar a partição de dados. Partição corrompida. Confirme para apagar e redefinir o dispositivo.</translation>
</message>
<message>
<source>Press confirm to erase all content and settings. Press cancel to resume boot.</source>
<translation type="unfinished"></translation>
<translation>Pressione confirmar para apagar todo o conteúdo e configurações. Pressione cancelar para voltar.</translation>
</message>
<message>
<source>Resetting device...
This may take up to a minute.</source>
<translation type="unfinished"></translation>
<translation>Redefinindo o dispositivo
Isso pode levar até um minuto.</translation>
</message>
</context>
<context>
@@ -1566,11 +1567,11 @@ This may take up to a minute.</source>
</message>
<message>
<source>No custom software found at this URL.</source>
<translation type="unfinished"></translation>
<translation>Não há software personalizado nesta URL.</translation>
</message>
<message>
<source>Something went wrong. Reboot the device.</source>
<translation type="unfinished"></translation>
<translation>Algo deu errado. Reinicie o dispositivo.</translation>
</message>
</context>
<context>
selfdrive/ui/translations/main_zh-CHT.ts
+10 -11
View File
@@ -95,7 +95,7 @@
</message>
<message>
<source>RAM PER</source>
<translation type="vanished">記憶體 %</translation>
<translation>記憶體 %</translation>
</message>
<message>
<source>OFF</source>
@@ -598,7 +598,7 @@ Reboot required.</source>
<translation type="vanished"> 當定速高於：</translation>
</message>
<message>
<source>when acc SET speed above the setting, it will switch to alternative controller.
<source>when acc SET speed above the setting, it will switch to alternative controller.
1 km/h = 0.62 mph</source>
<translation>當定速高於設定的速度時，將會啟用替代的控制器</translation>
</message>
@@ -1188,7 +1188,7 @@ Reboot now?</source>
</message>
<message>
<source>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.</source>
<translation>openpilot 需要將裝置固定在左右偏差 4° 以內，朝上偏差 5° 以内或朝下偏差 8° 以内。鏡頭在後台會持續自動校準，很少有需要重置的情况。</translation>
<translation>openpilot 需要將設備固定在左右偏差 4° 以內，朝上偏差 5° 以内或朝下偏差 8° 以内。鏡頭在後台會持續自動校準，很少有需要重置的情况。</translation>
</message>
<message>
<source> Your device is pointed %1° %2 and %3° %4.</source>
@@ -1584,17 +1584,16 @@ location set</source>
</message>
<message>
<source>Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device.</source>
<translation>無法掛載數據分區。分區可能已損壞。請按確認以清除並重置您的設備。</translation>
<translation>無法掛載資料分割區 分割區可能已經毀損 請確認是否要刪除並重新設定</translation>
</message>
<message>
<source>Press confirm to erase all content and settings. Press cancel to resume boot.</source>
<translation>按確認以清除所有內容和設置。按取消以繼續啟動。</translation>
<translation>按下確認以刪除所有內容及設定 按下取消來繼續開機</translation>
</message>
<message>
<source>Resetting device...
This may take up to a minute.</source>
<translation>正在重置設備...
這可能需要一分鐘的時間。</translation>
<translation>設備重置中 此過程可能需要幾分鐘</translation>
</message>
</context>
<context>
@@ -1720,11 +1719,11 @@ This may take up to a minute.</source>
</message>
<message>
<source>No custom software found at this URL.</source>
<translation>在此 URL 上未找到自定義軟件。</translation>
<translation>無法在此URL找到定制的軟體</translation>
</message>
<message>
<source>Something went wrong. Reboot the device.</source>
<translation>出現了問題。請重新啟動設備。</translation>
<translation>發生了一些錯誤 請重新啟動您的設備</translation>
</message>
</context>
<context>
@@ -2057,11 +2056,11 @@ This may take up to a minute.</source>
</message>
<message>
<source>openpilot longitudinal control may come in a future update.</source>
<translation></translation>
<translation>未來可能會推出openpilot縱向控制</translation>
</message>
<message>
<source>An experimental version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches.</source>
<translation></translation>
<translation>在非發行分支中 可找到包含實驗模式的openpilot縱向控制測試版本</translation>
</message>
<message>
<source>Enable experimental longitudinal control to allow Experimental mode.</source>
selfdrive/ui/update_translations.py
+1 -1
View File
@@ -22,7 +22,7 @@ def update_translations(vanish=False, plural_only=None, translations_dir=TRANSLA
for file in translation_files.values():
tr_file = os.path.join(translations_dir, f"{file}.ts")
args = f"lupdate -locations none -recursive {UI_DIR} -ts {tr_file}"
args = f"lupdate -locations none -recursive {UI_DIR} -ts {tr_file} -I {BASEDIR}"
if vanish:
args += " -no-obsolete"
if file in plural_only:

Some files were not shown because too many files have changed in this diff Show More