diff --git a/README.md b/README.md
index 071a92f05..eb6146b72 100755
--- a/README.md
+++ b/README.md
@@ -117,7 +117,7 @@ Documentation related to openpilot development can be found on [docs.comma.ai](h
You can add support for your car by following guides we have written for [Brand](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/) and [Model](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) ports. Generally, a car with adaptive cruise control and lane keep assist is a good candidate. [Join our Discord](https://discord.comma.ai) to discuss car ports: most car makes have a dedicated channel.
-Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs/).
+Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs#open-positions).
And [follow us on Twitter](https://twitter.com/comma_ai).
diff --git a/RELEASES.md b/RELEASES.md
index a4b2a7687..b2e7be396 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,6 @@
+Version 0.9.3 (2023-06-XX)
+========================
+
Version 0.9.2 (2023-05-22)
========================
* New driving model
diff --git a/cereal/car.capnp b/cereal/car.capnp
index 377afbea9..2730888f7 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -43,6 +43,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
overheat @19;
calibrationIncomplete @20;
calibrationInvalid @21;
+ calibrationRecalibrating @117;
controlsMismatch @22;
pcmEnable @23;
pcmDisable @24;
@@ -580,7 +581,7 @@ struct CarParams {
noOutput @19; # like silent but without silent CAN TXs
hondaBosch @20;
volkswagenPq @21;
- subaruLegacy @22; # pre-Global platform
+ subaruPreglobal @22; # pre-Global platform
hyundaiLegacy @23;
hyundaiCommunity @24;
volkswagenMlb @25;
diff --git a/cereal/dp.capnp b/cereal/dp.capnp
index 42a50398f..c367f641e 100644
--- a/cereal/dp.capnp
+++ b/cereal/dp.capnp
@@ -5,10 +5,7 @@ $Cxx.namespace("cereal");
# mapd
struct LiveMapData {
- speedLimitValid @0 :Bool;
- speedLimit @1 :Float32;
- speedLimitAheadValid @2 :Bool;
- speedLimitAhead @3 :Float32;
- speedLimitAheadDistance @4 :Float32;
- currentRoadName @5 :Text;
+ speedLimit @0 :Float32;
+ speedLimitValid @1 :Bool;
+ currentRoadName @2 :Text;
}
diff --git a/cereal/libcereal_shared.so b/cereal/libcereal_shared.so
index 1ded92a7f..07eea5ff0 100755
Binary files a/cereal/libcereal_shared.so and b/cereal/libcereal_shared.so differ
diff --git a/cereal/log.capnp b/cereal/log.capnp
index 0b37f9711..03dc212b3 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -591,7 +591,7 @@ struct RadarState @0x9a185389d6fdd05f {
}
struct LiveCalibrationData {
- calStatus @1 :Int8;
+ calStatus @11 :Status;
calCycle @2 :Int32;
calPerc @3 :Int8;
validBlocks @9 :Int32;
@@ -605,8 +605,16 @@ struct LiveCalibrationData {
wideFromDeviceEuler @10 :List(Float32);
warpMatrixDEPRECATED @0 :List(Float32);
+ calStatusDEPRECATED @1 :Int8;
warpMatrix2DEPRECATED @5 :List(Float32);
warpMatrixBigDEPRECATED @6 :List(Float32);
+
+ enum Status {
+ uncalibrated @0;
+ calibrated @1;
+ invalid @2;
+ recalibrating @3;
+ }
}
struct LiveTracks {
@@ -988,7 +996,7 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
aTargetMinDEPRECATED @4 :Float32;
aTargetMaxDEPRECATED @5 :Float32;
lateralValidDEPRECATED @0 :Bool;
- longitudinalValidDEPRECATED @2 :Bool;
+ longitudinalValid @2 :Bool;
dPolyDEPRECATED @1 :List(Float32);
laneWidthDEPRECATED @11 :Float32;
vCurvatureDEPRECATED @21 :Float32;
diff --git a/selfdrive/mapd/__init__.py b/cereal/messaging/impl_msgq-008e0027.os.tmp
similarity index 100%
rename from selfdrive/mapd/__init__.py
rename to cereal/messaging/impl_msgq-008e0027.os.tmp
diff --git a/cereal/visionipc/visionipc_pyx.so b/cereal/visionipc/visionipc_pyx.so
index 7444f11c4..2ea6fc9ec 100755
Binary files a/cereal/visionipc/visionipc_pyx.so and b/cereal/visionipc/visionipc_pyx.so differ
diff --git a/common/logging_extra.py b/common/logging_extra.py
index 5baaac1f9..899ad7a39 100644
--- a/common/logging_extra.py
+++ b/common/logging_extra.py
@@ -153,9 +153,9 @@ class SwagLogger(logging.Logger):
def bind_global(self, **kwargs):
self.global_ctx.update(kwargs)
- def event(self, event_name, *args, **kwargs):
+ def event(self, event, *args, **kwargs):
evt = NiceOrderedDict()
- evt['event'] = event_name
+ evt['event'] = event
if args:
evt['args'] = args
evt.update(kwargs)
diff --git a/common/params_pyx.so b/common/params_pyx.so
index f424e0844..a522165f6 100755
Binary files a/common/params_pyx.so and b/common/params_pyx.so differ
diff --git a/common/version.h b/common/version.h
index 8360a2ba5..c41423539 100644
--- a/common/version.h
+++ b/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "2023.05.19"
+#define COMMA_VERSION "2023.05.25"
diff --git a/docs/CARS.md b/docs/CARS.md
index d101c7dee..551bf42ae 100644
--- a/docs/CARS.md
+++ b/docs/CARS.md
@@ -6,260 +6,260 @@ A supported vehicle is one that just works when you install a comma three. All s
# 252 Supported Cars
-|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness|Video|
+|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|
Harness Kit
|Video|
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
-|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[](##)|[](##)|Honda Nidec||
-|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec||
-|Acura|RDX 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|Honda Bosch A||
-|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Buick|LaCrosse 2017-19[3](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[](##)|[](##)|OBD-II||
-|Cadillac|Escalade 2017[3](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[](##)|[](##)|OBD-II||
-|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[](##)|[](##)|OBD-II||
-|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[](##)|[](##)|GM|
|
-|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[](##)|[](##)|GM||
-|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[](##)|[](##)|GM||
-|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[1](#footnotes)|3 mph|6 mph|[](##)|[](##)|GM||
-|Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[](##)|[](##)|OBD-II|
|
-|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|FCA||
-|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|FCA||
-|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[](##)|[](##)|FCA||
-|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|FCA||
-|Chrysler|Pacifica Hybrid 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|FCA||
+|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Acura|RDX 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|View
- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Buick|LaCrosse 2017-19[3](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[](##)|[](##)|View
- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable ||
+|Cadillac|Escalade 2017[3](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[](##)|[](##)|View
- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable ||
+|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[](##)|[](##)|View
- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable ||
+|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[](##)|[](##)|View
- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[](##)|[](##)|View
- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[](##)|[](##)|View
- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[1](#footnotes)|3 mph|6 mph|[](##)|[](##)|View
- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[](##)|[](##)|View
- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable |
|
+|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|View
- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|View
- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[](##)|[](##)|View
- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|View
- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Chrysler|Pacifica Hybrid 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|View
- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
|comma|body|All|openpilot|0 mph|0 mph|[](##)|[](##)|None||
-|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[](##)|[](##)|Ford Q3||
-|Ford|Escape 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[](##)|[](##)|Ford Q3||
-|Ford|Explorer 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[](##)|[](##)|Ford Q3||
-|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|Stock|0 mph|0 mph|[](##)|[](##)|Ford Q3||
-|Ford|Maverick 2022-23|Co-Pilot360 Assist|Stock|0 mph|0 mph|[](##)|[](##)|Ford Q3||
-|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai F||
-|Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai F||
-|Genesis|G80 2017|All|Stock|19 mph|37 mph|[](##)|[](##)|Hyundai J||
-|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H||
-|Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai C||
-|Genesis|GV60 (Advanced Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A||
-|Genesis|GV60 (Performance Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai K||
-|Genesis|GV70 (2.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai L||
-|Genesis|GV70 (3.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai M||
-|Genesis|GV80 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai M||
-|GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[](##)|[](##)|OBD-II|
|
-|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[](##)|[](##)|GM|
|
-|Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|Honda Bosch A|
|
-|Honda|Accord Hybrid 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|Honda Bosch A||
-|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[](##)|[](##)|Honda Nidec|
|
-|Honda|Civic 2019-21|All|openpilot available[1](#footnotes)|0 mph|2 mph[4](#footnotes)|[](##)|[](##)|Honda Bosch A|
|
-|Honda|Civic 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Honda Bosch B|
|
-|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[](##)|[](##)|Honda Bosch A||
-|Honda|Civic Hatchback 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Honda Bosch B|
|
-|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec||
-|Honda|CR-V 2017-22|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[](##)|[](##)|Honda Bosch A||
-|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[](##)|[](##)|Honda Bosch A||
-|Honda|e 2020|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|Honda Bosch A||
-|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec||
-|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec||
-|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec||
-|Honda|HR-V 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Honda Bosch B||
-|Honda|Insight 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|Honda Bosch A||
-|Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|Honda Bosch A||
-|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[](##)|[](##)|Honda Nidec||
-|Honda|Passport 2019-21|All|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec||
-|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec||
-|Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|Honda Nidec||
-|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[](##)|[](##)|Hyundai B||
-|Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
-|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai E||
-|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai K|
|
-|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[](##)|[](##)|Hyundai J||
-|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai E||
-|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai Q||
-|Hyundai|Ioniq 5 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai Q||
-|Hyundai|Ioniq 5 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai K||
-|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai C||
-|Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H||
-|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai C||
-|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H||
-|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[](##)|[](##)|Hyundai C||
-|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai H||
-|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai B||
-|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai G||
-|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai O||
-|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai I|
|
-|Hyundai|Palisade 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
-|Hyundai|Santa Cruz 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai N||
-|Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai D||
-|Hyundai|Santa Fe 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai L|
|
-|Hyundai|Santa Fe Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai L||
-|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai L||
-|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai E||
-|Hyundai|Sonata 2020-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A|
|
-|Hyundai|Sonata Hybrid 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A||
-|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[](##)|[](##)|Hyundai L||
-|Hyundai|Tucson 2022[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai N||
-|Hyundai|Tucson 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai N||
-|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai L||
-|Hyundai|Tucson Hybrid 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai N||
-|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[](##)|[](##)|Hyundai E||
-|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|FCA|
|
-|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|FCA|
|
-|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai E||
-|Kia|EV6 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai P||
-|Kia|EV6 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai P||
-|Kia|EV6 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai L||
-|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai G||
-|Kia|Forte 2023|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai E||
-|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A||
-|Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A||
-|Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
-|Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai F|
|
-|Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
-|Kia|Niro EV 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai H|
|
-|Kia|Niro EV 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A||
-|Kia|Niro Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai F||
-|Kia|Niro Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A||
-|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[1](#footnotes)|10 mph|32 mph|[](##)|[](##)|Hyundai C||
-|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|32 mph|[](##)|[](##)|Hyundai D||
-|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[](##)|[](##)|Hyundai B||
-|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai G||
-|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai A||
-|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
-|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai E|
|
-|Kia|Sorento 2021-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai K||
-|Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai A||
-|Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|Hyundai N||
-|Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai N||
-|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai C|
|
-|Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai K||
-|Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|Hyundai H||
-|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|ES 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|ES Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|NX 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|NX Hybrid 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|RX 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|RX 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|RX Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Lincoln|Aviator 2021|Co-Pilot360 Plus|Stock|0 mph|0 mph|[](##)|[](##)|Ford Q3||
-|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|J533|
|
-|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|J533|
|
-|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[](##)|[](##)|Mazda||
-|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[](##)|[](##)|Mazda|
|
-|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|Nissan B||
-|Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|Nissan A|
|
-|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|Nissan A||
-|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|Nissan A||
-|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[](##)|[](##)|Ram||
-|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A||
-|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A|
|
-|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A||
-|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A||
-|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A||
-|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A||
-|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|Subaru B||
-|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|Subaru B||
-|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A|
|
-|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|Subaru A||
-|Škoda|Fabia 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[10](#footnotes)||
-|Škoda|Kamiq 2021[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[10](#footnotes)||
-|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Škoda|Kodiaq 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[10](#footnotes)||
-|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Avalon 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Avalon 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Avalon Hybrid 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|C-HR Hybrid 2021-22|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Camry 2021-23|All|openpilot|0 mph[6](#footnotes)|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Corolla 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Prius 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Prius Prime 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|RAV4 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|Toyota||
-|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Toyota|Sienna 2018-20|All|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|Toyota|
|
-|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
-|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
-|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
-|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|J533||
-|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|J533||
-|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533|
|
-|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|J533|
|
-|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|J533|
|
-|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|J533|
|
-|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Passat 2015-22[8](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[10](#footnotes)||
-|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[10](#footnotes)||
-|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[10](#footnotes)||
-|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533[10](#footnotes)||
-|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
-|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|J533||
+|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Ford|Escape 2020-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Ford|Explorer 2020-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Ford|Maverick 2022-23|Co-Pilot360 Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Genesis|G80 2017|All|Stock|19 mph|37 mph|[](##)|[](##)|View
- 1 Hyundai J connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Genesis|GV60 (Advanced Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Genesis|GV60 (Performance Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Genesis|GV70 (2.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Genesis|GV70 (3.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai M connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Genesis|GV80 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai M connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[](##)|[](##)|View
- 1 OBD-II connector
- 1 USB-C coupler
- 1 long OBD-C cable |
|
+|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[](##)|[](##)|View
- 1 GM connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|View
- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Honda|Accord Hybrid 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|View
- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Honda|Civic 2019-21|All|openpilot available[1](#footnotes)|0 mph|2 mph[4](#footnotes)|[](##)|[](##)|View
- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Honda|Civic 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Honda Bosch B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[](##)|[](##)|View
- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|Civic Hatchback 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Honda Bosch B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|CR-V 2017-22|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[](##)|[](##)|View
- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[](##)|[](##)|View
- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|e 2020|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|View
- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|HR-V 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Honda Bosch B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|Insight 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|View
- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[](##)|[](##)|View
- 1 Honda Bosch A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|Passport 2019-22|All|openpilot|25 mph|12 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|View
- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[](##)|[](##)|View
- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|View
- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[](##)|[](##)|View
- 1 Hyundai J connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|View
- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai Q connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Ioniq 5 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai Q connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Ioniq 5 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|View
- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|View
- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[](##)|[](##)|View
- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai G connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai O connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai I connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Hyundai|Palisade 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Hyundai|Santa Cruz 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai D connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Santa Fe 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Hyundai|Santa Fe Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Sonata 2020-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Hyundai|Sonata Hybrid 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Tucson 2022[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Tucson 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Tucson Hybrid 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|View
- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|View
- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|EV6 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai P connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|EV6 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai P connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|EV6 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai G connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Forte 2023|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Kia|Niro EV 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Kia|Niro EV 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Niro Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai F connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Niro Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[1](#footnotes)|10 mph|32 mph|[](##)|[](##)|View
- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|32 mph|[](##)|[](##)|View
- 1 Hyundai D connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[](##)|[](##)|View
- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai G connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai E connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Kia|Sorento 2021-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|ES 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|ES Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|NX 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|NX Hybrid 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|RX 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|RX 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|RX Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Lincoln|Aviator 2021|Co-Pilot360 Plus|openpilot available[1](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable |
|
+|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable |
|
+|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Mazda connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[](##)|[](##)|View
- 1 Mazda connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Nissan B connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Nissan A connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable |
|
+|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Nissan A connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Nissan A connector
- 1 RJ45 cable (7 ft)
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[](##)|[](##)|View
- 1 Ram connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Subaru B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Subaru B connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Subaru A connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Škoda|Fabia 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable [10](#footnotes)||
+|Škoda|Kamiq 2021[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable [10](#footnotes)||
+|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Škoda|Kodiaq 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable [10](#footnotes)||
+|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Avalon 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Avalon 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Avalon Hybrid 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|C-HR Hybrid 2021-22|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Camry 2021-23|All|openpilot|0 mph[6](#footnotes)|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Corolla 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Prius 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Prius Prime 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|RAV4 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box ||
+|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Toyota|Sienna 2018-20|All|openpilot available[2](#footnotes)|19 mph|0 mph|[](##)|[](##)|View
- 1 Toyota connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box |
|
+|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable |
|
+|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable |
|
+|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable |
|
+|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable |
|
+|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable |
|
+|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable |
|
+|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable |
|
+|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Passat 2015-22[8](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable [10](#footnotes)||
+|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable [10](#footnotes)||
+|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable [10](#footnotes)||
+|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable [10](#footnotes)||
+|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
+|Volkswagen|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[](##)|[](##)|View
- 1 J533 connector
- 1 USB-C coupler
- 1 harness box
- 1 long OBD-C cable ||
1Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`.
diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh
index 33aab9324..4c2e9aeef 100755
--- a/launch_chffrplus.sh
+++ b/launch_chffrplus.sh
@@ -193,10 +193,15 @@ function two_init {
mount -o remount,r /system
# osm server
- MODULE="osm-3s_v0.7.56"
- if [ ! -d /data/media/0/osm/ ]; then
- tar -vxf "/data/openpilot/selfdrive/mapd/assets/$MODULE.tar.xz" -C /data/media/0/
- mv "/data/media/0/$MODULE" /data/media/0/osm
+ if [ -f /data/params/d/dp_mapd ]; then
+ dp_mapd=`cat /data/params/d/dp_mapd`
+ if [ $dp_mapd == "1" ]; then
+ MODULE="osm-3s_v0.7.56"
+ if [ ! -d /data/media/0/osm/ ]; then
+ tar -vxf "/data/openpilot/system/hardware/eon/libs/$MODULE.tar.xz" -C /data/media/0/
+ mv "/data/media/0/$MODULE" /data/media/0/osm
+ fi
+ fi
fi
# Check for NEOS update
diff --git a/opendbc/can/libdbc.so b/opendbc/can/libdbc.so
index 71468dece..b07356b8f 100755
Binary files a/opendbc/can/libdbc.so and b/opendbc/can/libdbc.so differ
diff --git a/opendbc/can/parser_pyx.so b/opendbc/can/parser_pyx.so
index ed574ec4b..b144a295a 100755
Binary files a/opendbc/can/parser_pyx.so and b/opendbc/can/parser_pyx.so differ
diff --git a/panda/__init__.py b/panda/__init__.py
index 8b43e9938..d3a558f43 100644
--- a/panda/__init__.py
+++ b/panda/__init__.py
@@ -1,4 +1,4 @@
-from .python.constants import McuType, BASEDIR # noqa: F401
+from .python.constants import McuType, BASEDIR, FW_PATH # noqa: F401
from .python.serial import PandaSerial # noqa: F401
from .python import (Panda, PandaDFU, # noqa: F401
pack_can_buffer, unpack_can_buffer, calculate_checksum,
diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin
index e0f97495f..0e2240f45 100755
Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ
diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed
index 23ba5ec03..ea7bb2d62 100644
Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index bb8e6c901..227f76932 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -14,7 +14,7 @@ from typing import Optional
from itertools import accumulate
from .base import BaseHandle
-from .constants import McuType
+from .constants import FW_PATH, McuType
from .dfu import PandaDFU
from .isotp import isotp_send, isotp_recv
from .spi import PandaSpiHandle, PandaSpiException
@@ -151,7 +151,7 @@ class Panda:
SAFETY_NOOUTPUT = 19
SAFETY_HONDA_BOSCH = 20
SAFETY_VOLKSWAGEN_PQ = 21
- SAFETY_SUBARU_LEGACY = 22
+ SAFETY_SUBARU_PREGLOBAL = 22
SAFETY_HYUNDAI_LEGACY = 23
SAFETY_HYUNDAI_COMMUNITY = 24
SAFETY_STELLANTIS = 25
@@ -239,6 +239,8 @@ class Panda:
FLAG_GM_HW_CAM = 1
FLAG_GM_HW_CAM_LONG = 2
+ FLAG_FORD_LONG_CONTROL = 1
+
def __init__(self, serial: Optional[str] = None, claim: bool = True, disable_checks: bool = True):
self._connect_serial = serial
self._disable_checks = disable_checks
@@ -403,14 +405,16 @@ class Panda:
return []
def reset(self, enter_bootstub=False, enter_bootloader=False, reconnect=True):
+ # no response is expected since it resets right away
+ timeout = 5000 if isinstance(self._handle, PandaSpiHandle) else 15000
try:
if enter_bootloader:
- self._handle.controlWrite(Panda.REQUEST_IN, 0xd1, 0, 0, b'')
+ self._handle.controlWrite(Panda.REQUEST_IN, 0xd1, 0, 0, b'', timeout=timeout)
else:
if enter_bootstub:
- self._handle.controlWrite(Panda.REQUEST_IN, 0xd1, 1, 0, b'')
+ self._handle.controlWrite(Panda.REQUEST_IN, 0xd1, 1, 0, b'', timeout=timeout)
else:
- self._handle.controlWrite(Panda.REQUEST_IN, 0xd8, 0, 0, b'')
+ self._handle.controlWrite(Panda.REQUEST_IN, 0xd8, 0, 0, b'', timeout=timeout)
except Exception:
pass
if not enter_bootloader and reconnect:
@@ -485,7 +489,7 @@ class Panda:
def flash(self, fn=None, code=None, reconnect=True):
if not fn:
- fn = self._mcu_type.config.app_path
+ fn = os.path.join(FW_PATH, self._mcu_type.config.app_fn)
assert os.path.isfile(fn)
logging.debug("flash: main version is %s", self.get_version())
if not self.bootstub:
@@ -536,7 +540,8 @@ class Panda:
def up_to_date(self) -> bool:
current = self.get_signature()
- expected = Panda.get_signature_from_firmware(self.get_mcu_type().config.app_path)
+ fn = os.path.join(FW_PATH, self.get_mcu_type().config.app_fn)
+ expected = Panda.get_signature_from_firmware(fn)
return (current == expected)
def call_control_api(self, msg):
diff --git a/panda/python/constants.py b/panda/python/constants.py
index c55fd2c9b..4c3e778ad 100644
--- a/panda/python/constants.py
+++ b/panda/python/constants.py
@@ -3,7 +3,7 @@ import enum
from typing import List, NamedTuple
BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")
-
+FW_PATH = os.path.join(BASEDIR, "board/obj/")
class McuConfig(NamedTuple):
mcu: str
@@ -13,9 +13,9 @@ class McuConfig(NamedTuple):
sector_sizes: List[int]
serial_number_address: int
app_address: int
- app_path: str
+ app_fn: str
bootstub_address: int
- bootstub_path: str
+ bootstub_fn: str
Fx = (
0x1FFF7A10,
@@ -23,9 +23,9 @@ Fx = (
[0x4000 for _ in range(4)] + [0x10000] + [0x20000 for _ in range(11)],
0x1FFF79C0,
0x8004000,
- os.path.join(BASEDIR, "board", "obj", "panda.bin.signed"),
+ "panda.bin.signed",
0x8000000,
- os.path.join(BASEDIR, "board", "obj", "bootstub.panda.bin"),
+ "bootstub.panda.bin",
)
F2Config = McuConfig("STM32F2", 0x411, *Fx)
F4Config = McuConfig("STM32F4", 0x463, *Fx)
@@ -39,9 +39,9 @@ H7Config = McuConfig(
[0x20000 for _ in range(7)],
0x080FFFC0,
0x8020000,
- os.path.join(BASEDIR, "board", "obj", "panda_h7.bin.signed"),
+ "panda_h7.bin.signed",
0x8000000,
- os.path.join(BASEDIR, "board", "obj", "bootstub.panda_h7.bin"),
+ "bootstub.panda_h7.bin",
)
@enum.unique
diff --git a/panda/python/dfu.py b/panda/python/dfu.py
index f724bf15d..bebc243ce 100644
--- a/panda/python/dfu.py
+++ b/panda/python/dfu.py
@@ -1,3 +1,4 @@
+import os
import usb1
import struct
import binascii
@@ -6,7 +7,7 @@ from typing import List, Optional
from .base import BaseSTBootloaderHandle
from .spi import STBootloaderSPIHandle, PandaSpiException
from .usb import STBootloaderUSBHandle
-from .constants import McuType
+from .constants import FW_PATH, McuType
class PandaDFU:
@@ -110,10 +111,10 @@ class PandaDFU:
self._handle.erase_bootstub()
self._handle.erase_app()
self._handle.program(self._mcu_type.config.bootstub_address, code_bootstub)
- self.reset()
def recover(self):
- with open(self._mcu_type.config.bootstub_path, "rb") as f:
+ fn = os.path.join(FW_PATH, self._mcu_type.config.bootstub_fn)
+ with open(fn, "rb") as f:
code = f.read()
self.program_bootstub(code)
-
+ self.reset()
diff --git a/panda/python/spi.py b/panda/python/spi.py
index c753bf4dd..8a0fe5cc9 100644
--- a/panda/python/spi.py
+++ b/panda/python/spi.py
@@ -58,7 +58,14 @@ class SpiDevice:
"""
Provides locked, thread-safe access to a panda's SPI interface.
"""
- def __init__(self, speed):
+
+ # 50MHz is the max of the 845. older rev comma three
+ # may not support the full 50MHz
+ MAX_SPEED = 50000000
+
+ def __init__(self, speed=MAX_SPEED):
+ assert speed <= self.MAX_SPEED
+
if not os.path.exists(DEV_PATH):
raise PandaSpiUnavailable(f"SPI device not found: {DEV_PATH}")
if spidev is None:
@@ -87,9 +94,7 @@ class PandaSpiHandle(BaseHandle):
A class that mimics a libusb1 handle for panda SPI communications.
"""
def __init__(self):
- # 50MHz is the max of the 845. older rev comma three
- # may not support the full 50MHz
- self.dev = SpiDevice(50000000)
+ self.dev = SpiDevice()
# helpers
def _calc_checksum(self, data: List[int]) -> int:
@@ -115,8 +120,11 @@ class PandaSpiHandle(BaseHandle):
logging.debug("starting transfer: endpoint=%d, max_rx_len=%d", endpoint, max_rx_len)
logging.debug("==============================================")
+ n = 0
+ start_time = time.monotonic()
exc = PandaSpiException()
- for n in range(MAX_XFER_RETRY_COUNT):
+ while (time.monotonic() - start_time) < timeout*1e-3:
+ n += 1
logging.debug("\ntry #%d", n+1)
try:
logging.debug("- send header")
@@ -124,16 +132,18 @@ class PandaSpiHandle(BaseHandle):
packet += bytes([reduce(lambda x, y: x^y, packet) ^ CHECKSUM_START])
spi.xfer2(packet)
+ to = timeout - (time.monotonic() - start_time)*1e3
logging.debug("- waiting for header ACK")
- self._wait_for_ack(spi, HACK, timeout, 0x11)
+ self._wait_for_ack(spi, HACK, int(to), 0x11)
# send data
logging.debug("- sending data")
packet = bytes([*data, self._calc_checksum(data)])
spi.xfer2(packet)
+ to = timeout - (time.monotonic() - start_time)*1e3
logging.debug("- waiting for data ACK")
- self._wait_for_ack(spi, DACK, timeout, 0x13)
+ self._wait_for_ack(spi, DACK, int(to), 0x13)
# get response length, then response
response_len_bytes = bytes(spi.xfer2(b"\x00" * 2))
@@ -149,7 +159,7 @@ class PandaSpiHandle(BaseHandle):
return dat[:-1]
except PandaSpiException as e:
exc = e
- logging.debug("SPI transfer failed, %d retries left", MAX_XFER_RETRY_COUNT - n - 1, exc_info=True)
+ logging.debug("SPI transfer failed, retrying", exc_info=True)
raise exc
# libusb1 functions
diff --git a/rednose/helpers/ekf_sym_pyx.so b/rednose/helpers/ekf_sym_pyx.so
index be18b8c1f..3b4ab2d52 100755
Binary files a/rednose/helpers/ekf_sym_pyx.so and b/rednose/helpers/ekf_sym_pyx.so differ
diff --git a/selfdrive/boardd/boardd b/selfdrive/boardd/boardd
index 62d2b53cf..929eba081 100755
Binary files a/selfdrive/boardd/boardd and b/selfdrive/boardd/boardd differ
diff --git a/selfdrive/boardd/boardd_api_impl.so b/selfdrive/boardd/boardd_api_impl.so
index d9766f707..1616fa33c 100755
Binary files a/selfdrive/boardd/boardd_api_impl.so and b/selfdrive/boardd/boardd_api_impl.so differ
diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h
index f93d29217..b07aec509 100644
--- a/selfdrive/boardd/panda_comms.h
+++ b/selfdrive/boardd/panda_comms.h
@@ -74,9 +74,9 @@ private:
uint8_t rx_buf[SPI_BUF_SIZE];
inline static std::recursive_mutex hw_lock;
- int wait_for_ack(spi_ioc_transfer &transfer, uint8_t ack);
- int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len);
- int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len);
- int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len);
+ int wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout);
+ int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout);
+ int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
+ int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
};
#endif
diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py
index aa47617ff..44b84577c 100755
--- a/selfdrive/boardd/pandad.py
+++ b/selfdrive/boardd/pandad.py
@@ -7,7 +7,7 @@ import subprocess
from typing import List, NoReturn
from functools import cmp_to_key
-from panda import Panda, PandaDFU
+from panda import Panda, PandaDFU, FW_PATH
from common.basedir import BASEDIR
from common.params import Params
from system.hardware import HARDWARE
@@ -16,7 +16,8 @@ from system.swaglog import cloudlog
def get_expected_signature(panda: Panda) -> bytes:
try:
- return Panda.get_signature_from_firmware(panda.get_mcu_type().config.app_path)
+ fn = os.path.join(FW_PATH, panda.get_mcu_type().config.app_fn)
+ return Panda.get_signature_from_firmware(fn)
except Exception:
cloudlog.exception("Error computing expected signature")
return b""
diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py
index 4be5b3f7e..d0504d6bf 100755
--- a/selfdrive/boardd/tests/test_boardd_loopback.py
+++ b/selfdrive/boardd/tests/test_boardd_loopback.py
@@ -1,9 +1,11 @@
#!/usr/bin/env python3
import os
+import copy
import random
import time
import unittest
from collections import defaultdict
+from pprint import pprint
import cereal.messaging as messaging
from cereal import car, log
@@ -59,16 +61,18 @@ class TestBoardd(unittest.TestCase):
sendcan = messaging.pub_sock('sendcan')
can = messaging.sub_sock('can', conflate=False, timeout=100)
+ sm = messaging.SubMaster(['pandaStates'])
time.sleep(0.5)
n = 200
for i in range(n):
+ print(f"boardd loopback {i}/{n}")
self.spinner.update(f"boardd loopback {i}/{n}")
sent_msgs = defaultdict(set)
- for _ in range(random.randrange(10)):
+ for _ in range(random.randrange(20, 100)):
to_send = []
- for __ in range(random.randrange(100)):
+ for __ in range(random.randrange(20)):
bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3])
addr = random.randrange(1, 1<<29)
dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9)))
@@ -76,21 +80,29 @@ class TestBoardd(unittest.TestCase):
to_send.append(make_can_msg(addr, dat, bus))
sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan'))
- for _ in range(100 * 2):
+ sent_loopback = copy.deepcopy(sent_msgs)
+ sent_loopback.update({k+128: copy.deepcopy(v) for k, v in sent_msgs.items()})
+ sent_total = {k: len(v) for k, v in sent_loopback.items()}
+ for _ in range(100 * 5):
+ sm.update(0)
recvd = messaging.drain_sock(can, wait_for_one=True)
for msg in recvd:
for m in msg.can:
- if m.src >= 128:
- key = (m.address, m.dat)
- assert key in sent_msgs[m.src-128], f"got unexpected msg: {m.src=} {m.address=} {m.dat=}"
- sent_msgs[m.src-128].discard(key)
+ key = (m.address, m.dat)
+ assert key in sent_loopback[m.src], f"got unexpected msg: {m.src=} {m.address=} {m.dat=}"
+ sent_loopback[m.src].discard(key)
- if all(len(v) == 0 for v in sent_msgs.values()):
+ if all(len(v) == 0 for v in sent_loopback.values()):
break
# if a set isn't empty, messages got dropped
- for bus in sent_msgs.keys():
- assert not len(sent_msgs[bus]), f"loop {i}: bus {bus} missing {len(sent_msgs[bus])} messages"
+ pprint(sent_msgs)
+ pprint(sent_loopback)
+ print({k: len(x) for k, x in sent_loopback.items()})
+ print(sum([len(x) for x in sent_loopback.values()]))
+ pprint(sm['pandaStates']) # may drop messages due to RX buffer overflow
+ for bus in sent_loopback.keys():
+ assert not len(sent_loopback[bus]), f"loop {i}: bus {bus} missing {len(sent_loopback[bus])} out of {sent_total[bus]} messages"
if __name__ == "__main__":
diff --git a/selfdrive/camerad/camerad b/selfdrive/camerad/camerad
index 60267655e..d9ad79da1 100755
Binary files a/selfdrive/camerad/camerad and b/selfdrive/camerad/camerad differ
diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py
index a9d1a528c..17ba59f96 100644
--- a/selfdrive/car/chrysler/values.py
+++ b/selfdrive/car/chrysler/values.py
@@ -1,12 +1,11 @@
from enum import IntFlag
from dataclasses import dataclass
-from enum import Enum
from typing import Dict, List, Optional, Union
from cereal import car
from panda.python import uds
from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarInfo, Harness
+from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
Ecu = car.CarParams.Ecu
@@ -61,7 +60,7 @@ RAM_CARS = RAM_DT | RAM_HD
@dataclass
class ChryslerCarInfo(CarInfo):
package: str = "Adaptive Cruise Control (ACC)"
- harness: Enum = Harness.fca
+ harness_kit: HarnessKit = HarnessKit(Harness.fca)
CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = {
@@ -75,10 +74,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-23", harness=Harness.ram),
+ CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-23", harness_kit=HarnessKit(Harness.ram)),
CAR.RAM_HD: [
- ChryslerCarInfo("Ram 2500 2020-22", harness=Harness.ram),
- ChryslerCarInfo("Ram 3500 2019-22", harness=Harness.ram),
+ ChryslerCarInfo("Ram 2500 2020-22", harness_kit=HarnessKit(Harness.ram)),
+ ChryslerCarInfo("Ram 3500 2019-22", harness_kit=HarnessKit(Harness.ram)),
],
}
@@ -227,6 +226,7 @@ FW_VERSIONS = {
b'68453514AD',
b'68510283AG',
b'68527375AD',
+ b'68527346AE',
],
(Ecu.srs, 0x744, None): [
b'68428609AB',
@@ -249,6 +249,7 @@ FW_VERSIONS = {
b'68535469AB',
b'68535470AC',
b'68586307AB',
+ b'68548900AB',
],
(Ecu.fwdRadar, 0x753, None): [
b'04672892AB',
@@ -276,6 +277,7 @@ FW_VERSIONS = {
b'68552788AA',
b'68552790AA',
b'68585112AB',
+ b'68552789AA',
],
(Ecu.engine, 0x7e0, None): [
b'05036065AE ',
@@ -287,6 +289,7 @@ FW_VERSIONS = {
b'68500630AD',
b'68500630AE',
b'68539650AD',
+ b'05149846AA ',
],
(Ecu.transmission, 0x7e1, None): [
b'68360078AL',
@@ -299,6 +302,7 @@ FW_VERSIONS = {
b'68484467AC',
b'68502994AD',
b'68540431AB',
+ b'68520867AE',
],
},
diff --git a/selfdrive/car/disable_ecu.py b/selfdrive/car/disable_ecu.py
index cd3e93fa8..ed98e14dc 100755
--- a/selfdrive/car/disable_ecu.py
+++ b/selfdrive/car/disable_ecu.py
@@ -31,8 +31,8 @@ def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, com_cont_req=b'\x28\x83\x01'
except Exception:
cloudlog.exception("ecu disable exception")
- print(f"ecu disable retry ({i+1}) ...")
- cloudlog.warning("ecu disable failed")
+ cloudlog.error(f"ecu disable retry ({i + 1}) ...")
+ cloudlog.error("ecu disable failed")
return False
diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py
index fa95b03fd..bfb182c5e 100644
--- a/selfdrive/car/docs_definitions.py
+++ b/selfdrive/car/docs_definitions.py
@@ -1,5 +1,6 @@
import re
from collections import namedtuple
+import copy
from dataclasses import dataclass, field
from enum import Enum
from typing import Dict, List, Optional, Tuple, Union
@@ -20,7 +21,7 @@ class Column(Enum):
FSR_STEERING = "No ALC below"
STEERING_TORQUE = "Steering Torque"
AUTO_RESUME = "Resume from stop"
- HARNESS = "Harness"
+ HARNESS = "Harness Kit"
VIDEO = "Video"
@@ -69,6 +70,23 @@ class Harness(Enum):
none = "None"
+class HarnessPart(Enum):
+ harness_box = "harness box"
+ comma_power_v2 = "comma power v2"
+ rj45_cable = "RJ45 cable (7 ft)"
+ long_obdc_cable = "long OBD-C cable"
+ usbc_coupler = "USB-C coupler"
+
+
+DEFAULT_HARNESS_PARTS: List[HarnessPart] = [HarnessPart.harness_box, HarnessPart.comma_power_v2, HarnessPart.rj45_cable]
+
+
+@dataclass
+class HarnessKit:
+ connector: Harness = Harness.none
+ parts: List[HarnessPart] = field(default_factory=lambda: copy.copy(DEFAULT_HARNESS_PARTS))
+
+
CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only", "shop_footnote"], defaults=(False, False))
@@ -135,7 +153,9 @@ class CarInfo:
footnotes: List[Enum] = field(default_factory=list)
min_steer_speed: Optional[float] = None
min_enable_speed: Optional[float] = None
- harness: Enum = Harness.none
+
+ # harness connectors + all the parts needed
+ harness_kit: HarnessKit = HarnessKit()
def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]):
self.car_name = CP.carName
@@ -165,12 +185,14 @@ class CarInfo:
self.min_enable_speed = CP.minEnableSpeed
# harness column
- harness_col = self.harness.value
- if self.harness is not Harness.none:
+ harness_col = self.harness_kit.connector.value
+ if self.harness_kit.connector is not Harness.none:
model_years = self.model + (' ' + self.years if self.years else '')
- harness_col = f'{harness_col}'
+ harness_connector = f'- 1 {harness_col} connector'
+ harness_parts = '
'.join([f"- {self.harness_kit.parts.count(part)} {part.value}" for part in sorted(set(self.harness_kit.parts), key=lambda part: part.value)])
+ harness_col = f'View
{harness_connector}
{harness_parts} '
- self.row = {
+ self.row: Dict[Enum, Union[str, Star]] = {
Column.MAKE: self.make,
Column.MODEL: self.model,
Column.PACKAGE: self.package,
diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
index 29b86f4ff..d9a9ae6bc 100644
--- a/selfdrive/car/ford/carcontroller.py
+++ b/selfdrive/car/ford/carcontroller.py
@@ -82,15 +82,14 @@ class CarController:
### longitudinal control ###
# send acc msg at 50Hz
if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
+ # Both gas and accel are in m/s^2, accel is used solely for braking
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
-
gas = accel
- decel = accel < 0.0
- if accel < -0.5:
- gas = -5.0
+ if not CC.longActive or gas < CarControllerParams.MIN_GAS:
+ gas = CarControllerParams.INACTIVE_GAS
stopping = CC.actuators.longControlState == LongCtrlState.stopping
- can_sends.append(create_acc_msg(self.packer, CC.longActive, gas, accel, decel, stopping))
+ can_sends.append(create_acc_msg(self.packer, CC.longActive, gas, accel, stopping))
### ui ###
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index 4ab968ea9..9be2c7637 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -75,7 +75,7 @@ class CarState(CarStateBase):
# safety
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
- ret.stockAeb = ret.stockFcw and ret.cruiseState.enabled
+ ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"])
# button presses
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
@@ -217,6 +217,8 @@ class CarState(CarStateBase):
def get_cam_can_parser(CP):
signals = [
# sig_name, sig_address
+ ("CmbbBrkDecel_B_Rq", "ACCDATA_2"), # AEB actuation request bit
+
("HaDsply_No_Cs", "ACCDATA_3"),
("HaDsply_No_Cnt", "ACCDATA_3"),
("AccStopStat_D_Dsply", "ACCDATA_3"), # ACC stopped status message
@@ -261,6 +263,7 @@ class CarState(CarStateBase):
checks = [
# sig_address, frequency
+ ("ACCDATA_2", 50),
("ACCDATA_3", 5),
("IPMA_Data", 1),
]
diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py
index daf880aa8..97a8c025d 100644
--- a/selfdrive/car/ford/fordcan.py
+++ b/selfdrive/car/ford/fordcan.py
@@ -101,7 +101,7 @@ def create_lat_ctl2_msg(packer, mode: int, path_offset: float, path_angle: float
return packer.make_can_msg("LateralMotionControl2", CANBUS.main, values)
-def create_acc_msg(packer, long_active: bool, gas: float, accel: float, decel: bool, stopping: bool):
+def create_acc_msg(packer, long_active: bool, gas: float, accel: float, stopping: bool):
"""
Creates a CAN message for the Ford ACC Command.
@@ -111,11 +111,13 @@ def create_acc_msg(packer, long_active: bool, gas: float, accel: float, decel: b
Frequency is 50Hz.
"""
+ decel = accel < 0 and long_active
values = {
"AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2
"Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes
"AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2
"AccResumEnbl_B_Rq": 1 if long_active else 0,
+ # TODO: we may be able to improve braking response by utilizing pre-charging better
"AccBrkPrchg_B_Rq": 1 if decel else 0, # Pre-charge brake request: 0=No, 1=Yes
"AccBrkDecel_B_Rq": 1 if decel else 0, # Deceleration request: 0=Inactive, 1=Active
"AccStopStat_B_Rq": 1 if stopping else 0,
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index 937980899..626853dd8 100644
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
from cereal import car
+from panda import Panda
from common.conversions import Conversions as CV
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car.ford.values import CAR, Ecu
@@ -25,6 +26,11 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.2
ret.steerLimitTimer = 1.0
+ ret.experimentalLongitudinalAvailable = True
+ if experimental_long:
+ ret.safetyConfigs[0].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL
+ ret.openpilotLongitudinalControl = True
+
if candidate == CAR.BRONCO_SPORT_MK1:
ret.wheelbase = 2.67
ret.steerRatio = 17.7
diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
index 1ec34b219..6425179a2 100644
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@@ -1,11 +1,10 @@
from collections import defaultdict
from dataclasses import dataclass
-from enum import Enum
from typing import Dict, List, Set, Union
from cereal import car
from selfdrive.car import AngleRateLimit, dbc_dict
-from selfdrive.car.docs_definitions import CarInfo, Harness
+from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
@@ -32,6 +31,8 @@ class CarControllerParams:
ACCEL_MAX = 2.0 # m/s^s max acceleration
ACCEL_MIN = -3.5 # m/s^s max deceleration
+ MIN_GAS = -0.5
+ INACTIVE_GAS = -5.0
def __init__(self, CP):
pass
@@ -65,7 +66,7 @@ DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base
@dataclass
class FordCarInfo(CarInfo):
package: str = "Co-Pilot360 Assist+"
- harness: Enum = Harness.ford_q3
+ harness_kit: HarnessKit = HarnessKit(Harness.ford_q3)
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
@@ -156,11 +157,13 @@ FW_VERSIONS = {
},
CAR.EXPLORER_MK6: {
(Ecu.eps, 0x730, None): [
+ b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.abs, 0x760, None): [
+ b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -170,11 +173,13 @@ FW_VERSIONS = {
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x706, None): [
+ b'LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.engine, 0x7E0, None): [
+ b'LB5A-14C204-ATJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-BUJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'MB5A-14C204-MD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py
index 63189bcd8..0de206667 100644
--- a/selfdrive/car/gm/gmcan.py
+++ b/selfdrive/car/gm/gmcan.py
@@ -20,10 +20,20 @@ def create_buttons(packer, bus, idx, button):
def create_pscm_status(packer, bus, pscm_status):
- checksum_mod = int(1 - pscm_status["HandsOffSWlDetectionStatus"]) << 5
- pscm_status["HandsOffSWlDetectionStatus"] = 1
- pscm_status["PSCMStatusChecksum"] += checksum_mod
- return packer.make_can_msg("PSCMStatus", bus, pscm_status)
+ values = {s: pscm_status[s] for s in [
+ "HandsOffSWDetectionMode",
+ "HandsOffSWlDetectionStatus",
+ "LKATorqueDeliveredStatus",
+ "LKADriverAppldTrq",
+ "LKATorqueDelivered",
+ "LKATotalTorqueDelivered",
+ "RollingCounter",
+ "PSCMStatusChecksum",
+ ]}
+ checksum_mod = int(1 - values["HandsOffSWlDetectionStatus"]) << 5
+ values["HandsOffSWlDetectionStatus"] = 1
+ values["PSCMStatusChecksum"] += checksum_mod
+ return packer.make_can_msg("PSCMStatus", bus, values)
def create_steering_control(packer, bus, apply_steer, idx, lkas_active):
diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py
index bc2858a66..865ba8ed0 100644
--- a/selfdrive/car/gm/values.py
+++ b/selfdrive/car/gm/values.py
@@ -5,7 +5,7 @@ from typing import Dict, List, Union
from cereal import car
from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness
+from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit, HarnessPart
Ecu = car.CarParams.Ecu
@@ -89,9 +89,9 @@ class GMCarInfo(CarInfo):
def init_make(self, CP: car.CarParams):
if CP.networkLocation == car.CarParams.NetworkLocation.fwdCamera:
- self.harness = Harness.gm
+ self.harness_kit = HarnessKit(Harness.gm)
else:
- self.harness = Harness.obd_ii
+ self.harness_kit = HarnessKit(Harness.obd_ii, parts=[HarnessPart.long_obdc_cable, HarnessPart.usbc_coupler])
self.footnotes.append(Footnote.OBD_II)
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 951d3c821..05c6f2543 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -6,7 +6,7 @@ from cereal import car
from common.conversions import Conversions as CV
from panda.python import uds
from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness
+from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
Ecu = car.CarParams.Ecu
@@ -110,9 +110,9 @@ class HondaCarInfo(CarInfo):
def init_make(self, CP: car.CarParams):
if CP.carFingerprint in HONDA_BOSCH:
- self.harness = Harness.bosch_b if CP.carFingerprint in HONDA_BOSCH_RADARLESS else Harness.bosch_a
+ self.harness_kit = HarnessKit(Harness.bosch_b) if CP.carFingerprint in HONDA_BOSCH_RADARLESS else HarnessKit(Harness.bosch_a)
else:
- self.harness = Harness.nidec
+ self.harness_kit = HarnessKit(Harness.nidec)
CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = {
@@ -146,7 +146,7 @@ CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = {
CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
CAR.PILOT: [
HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS),
- HondaCarInfo("Honda Passport 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS),
+ HondaCarInfo("Honda Passport 2019-22", "All", min_steer_speed=12. * CV.MPH_TO_MS),
],
CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-23", min_steer_speed=12. * CV.MPH_TO_MS),
CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
@@ -1114,6 +1114,7 @@ FW_VERSIONS = {
b'28101-5EZ-A210\x00\x00',
b'28101-5EZ-A600\x00\x00',
b'28101-5EZ-A430\x00\x00',
+ b'28101-5EZ-A700\x00\x00',
],
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
b'37805-RLV-4060\x00\x00',
@@ -1128,6 +1129,7 @@ FW_VERSIONS = {
b'37805-RLV-B220\x00\x00',
b'37805-RLV-B210\x00\x00',
b'37805-RLV-L160\x00\x00',
+ b'37805-RLV-B420\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-TG7-A030\x00\x00',
@@ -1141,6 +1143,7 @@ FW_VERSIONS = {
b'39990-TG7-A060\x00\x00',
b'39990-TG7-A070\x00\x00',
b'39990-TGS-A230\x00\x00',
+ b'39990-TGS-A320\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'36161-TG7-A310\x00\x00',
@@ -1161,6 +1164,7 @@ FW_VERSIONS = {
b'36161-TGT-A030\x00\x00',
b'36161-TGT-A130\x00\x00',
b'36161-TGS-A030\x00\x00',
+ b'36161-TGS-A220\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TG7-A020\x00\x00',
@@ -1168,6 +1172,7 @@ FW_VERSIONS = {
b'77959-TG7-A210\x00\x00',
b'77959-TG7-Y210\x00\x00',
b'77959-TGS-A010\x00\x00',
+ b'77959-TGS-A110\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TG7-A040\x00\x00',
@@ -1201,6 +1206,7 @@ FW_VERSIONS = {
b'78109-TGS-AT20\x00\x00',
b'78109-TGS-AX20\x00\x00',
b'78109-TGS-AJ20\x00\x00',
+ b'78109-TGS-AC10\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TG7-A130\x00\x00',
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index ddd877245..b5d35cc9e 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -6,7 +6,7 @@ from cereal import car
from panda.python import uds
from common.conversions import Conversions as CV
from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness
+from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
Ecu = car.CarParams.Ecu
@@ -147,114 +147,114 @@ class HyundaiCarInfo(CarInfo):
CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
CAR.ELANTRA: [
- HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_b),
- HyundaiCarInfo("Hyundai Elantra GT 2017-19", harness=Harness.hyundai_e),
- HyundaiCarInfo("Hyundai i30 2017-19", harness=Harness.hyundai_e),
+ HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_b)),
+ HyundaiCarInfo("Hyundai Elantra GT 2017-19", harness_kit=HarnessKit(Harness.hyundai_e)),
+ HyundaiCarInfo("Hyundai i30 2017-19", harness_kit=HarnessKit(Harness.hyundai_e)),
],
- CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k),
- CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k),
+ CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness_kit=HarnessKit(Harness.hyundai_k)),
+ CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", harness_kit=HarnessKit(Harness.hyundai_k)),
CAR.HYUNDAI_GENESIS: [
- HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j), # TODO: check 2015 packages
- HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j),
+ HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_j)), # TODO: check 2015 packages
+ HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_j)),
],
- CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c),
- CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", harness=Harness.hyundai_h), # TODO: confirm 2020-21 harness
- CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness=Harness.hyundai_c),
- CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", harness=Harness.hyundai_h),
- CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", harness=Harness.hyundai_c),
- CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", harness=Harness.hyundai_h),
- CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", harness=Harness.hyundai_b),
- CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", harness=Harness.hyundai_g),
- CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022", harness=Harness.hyundai_o),
- CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo", harness=Harness.hyundai_i), # TODO: check packages
- CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", harness=Harness.hyundai_d),
- CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", video_link="https://youtu.be/VnHzSTygTS4", harness=Harness.hyundai_l),
- CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All", harness=Harness.hyundai_l),
- CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", harness=Harness.hyundai_l),
- CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a),
- CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness=Harness.hyundai_e),
+ CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness_kit=HarnessKit(Harness.hyundai_c)),
+ CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", harness_kit=HarnessKit(Harness.hyundai_h)), # TODO: confirm 2020-21 harness
+ CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness_kit=HarnessKit(Harness.hyundai_c)),
+ CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", harness_kit=HarnessKit(Harness.hyundai_h)),
+ CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", harness_kit=HarnessKit(Harness.hyundai_c)),
+ CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", harness_kit=HarnessKit(Harness.hyundai_h)),
+ CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", harness_kit=HarnessKit(Harness.hyundai_b)),
+ CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", harness_kit=HarnessKit(Harness.hyundai_g)),
+ CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022", harness_kit=HarnessKit(Harness.hyundai_o)),
+ CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/0dwpAHiZgFo", harness_kit=HarnessKit(Harness.hyundai_i)), # TODO: check packages
+ CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", harness_kit=HarnessKit(Harness.hyundai_d)),
+ CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", video_link="https://youtu.be/VnHzSTygTS4", harness_kit=HarnessKit(Harness.hyundai_l)),
+ CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All", harness_kit=HarnessKit(Harness.hyundai_l)),
+ CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", harness_kit=HarnessKit(Harness.hyundai_l)),
+ CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", harness_kit=HarnessKit(Harness.hyundai_a)),
+ CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness_kit=HarnessKit(Harness.hyundai_e)),
CAR.TUCSON: [
- HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_l),
- HyundaiCarInfo("Hyundai Tucson Diesel 2019", harness=Harness.hyundai_l),
+ HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_l)),
+ HyundaiCarInfo("Hyundai Tucson Diesel 2019", harness_kit=HarnessKit(Harness.hyundai_l)),
],
CAR.PALISADE: [
- HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", harness=Harness.hyundai_h),
- HyundaiCarInfo("Kia Telluride 2020-22", "All", harness=Harness.hyundai_h),
+ HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", harness_kit=HarnessKit(Harness.hyundai_h)),
+ HyundaiCarInfo("Kia Telluride 2020-22", "All", harness_kit=HarnessKit(Harness.hyundai_h)),
],
- CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, harness=Harness.hyundai_e),
- CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", harness=Harness.hyundai_a),
+ CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_e)),
+ CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", harness_kit=HarnessKit(Harness.hyundai_a)),
CAR.IONIQ_5: [
- HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", harness=Harness.hyundai_q),
- HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", harness=Harness.hyundai_k),
- HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", harness=Harness.hyundai_q),
+ HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_q)),
+ HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", harness_kit=HarnessKit(Harness.hyundai_k)),
+ HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", harness_kit=HarnessKit(Harness.hyundai_q)),
],
CAR.TUCSON_4TH_GEN: [
- HyundaiCarInfo("Hyundai Tucson 2022", harness=Harness.hyundai_n),
- HyundaiCarInfo("Hyundai Tucson 2023", "All", harness=Harness.hyundai_n),
+ HyundaiCarInfo("Hyundai Tucson 2022", harness_kit=HarnessKit(Harness.hyundai_n)),
+ HyundaiCarInfo("Hyundai Tucson 2023", "All", harness_kit=HarnessKit(Harness.hyundai_n)),
],
- CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022-23", "All", harness=Harness.hyundai_n),
- CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2022-23", harness=Harness.hyundai_n),
+ CAR.TUCSON_HYBRID_4TH_GEN: HyundaiCarInfo("Hyundai Tucson Hybrid 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_n)),
+ CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2022-23", harness_kit=HarnessKit(Harness.hyundai_n)),
# Kia
CAR.KIA_FORTE: [
- HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g),
- HyundaiCarInfo("Kia Forte 2023", harness=Harness.hyundai_e),
+ HyundaiCarInfo("Kia Forte 2019-21", harness_kit=HarnessKit(Harness.hyundai_g)),
+ HyundaiCarInfo("Kia Forte 2023", harness_kit=HarnessKit(Harness.hyundai_e)),
],
- CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", harness=Harness.hyundai_a),
- CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020", harness=Harness.hyundai_a),
+ CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", harness_kit=HarnessKit(Harness.hyundai_a)),
+ CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020", harness_kit=HarnessKit(Harness.hyundai_a)),
CAR.KIA_NIRO_EV: [
- HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h),
- HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f),
- HyundaiCarInfo("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c),
- HyundaiCarInfo("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h),
+ HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_h)),
+ HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_f)),
+ HyundaiCarInfo("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_c)),
+ HyundaiCarInfo("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness_kit=HarnessKit(Harness.hyundai_h)),
],
- CAR.KIA_NIRO_EV_2ND_GEN: HyundaiCarInfo("Kia Niro EV 2023", "All", harness=Harness.hyundai_a),
+ CAR.KIA_NIRO_EV_2ND_GEN: HyundaiCarInfo("Kia Niro EV 2023", "All", harness_kit=HarnessKit(Harness.hyundai_a)),
CAR.KIA_NIRO_PHEV: [
- HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c),
- HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", harness=Harness.hyundai_d),
+ HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, harness_kit=HarnessKit(Harness.hyundai_c)),
+ HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", harness_kit=HarnessKit(Harness.hyundai_d)),
],
CAR.KIA_NIRO_HEV_2021: [
- HyundaiCarInfo("Kia Niro Hybrid 2021-22", harness=Harness.hyundai_f), # TODO: 2021 could be hyundai_d, verify
+ HyundaiCarInfo("Kia Niro Hybrid 2021-22", harness_kit=HarnessKit(Harness.hyundai_f)), # TODO: 2021 could be hyundai_d, verify
],
- CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", harness=Harness.hyundai_a),
- CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness=Harness.hyundai_b), # TODO: may support 2016, 2018
- CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness=Harness.hyundai_g),
+ CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", harness_kit=HarnessKit(Harness.hyundai_a)),
+ CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", harness_kit=HarnessKit(Harness.hyundai_b)), # TODO: may support 2016, 2018
+ CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", harness_kit=HarnessKit(Harness.hyundai_g)),
CAR.KIA_OPTIMA_H: [
HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control"), # TODO: may support adjacent years
HyundaiCarInfo("Kia Optima Hybrid 2019"),
],
- CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness=Harness.hyundai_a),
- CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", harness=Harness.hyundai_n),
+ CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness_kit=HarnessKit(Harness.hyundai_a)),
+ CAR.KIA_SPORTAGE_5TH_GEN: HyundaiCarInfo("Kia Sportage 2023", harness_kit=HarnessKit(Harness.hyundai_n)),
CAR.KIA_SORENTO: [
- HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", video_link="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),
+ HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness_kit=HarnessKit(Harness.hyundai_c)),
+ HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness_kit=HarnessKit(Harness.hyundai_e)),
],
- 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),
- CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022", "All", harness=Harness.hyundai_k),
- CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness=Harness.hyundai_e),
+ CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", harness_kit=HarnessKit(Harness.hyundai_k)),
+ CAR.KIA_SORENTO_PHEV_4TH_GEN: HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", harness_kit=HarnessKit(Harness.hyundai_a)),
+ CAR.KIA_SPORTAGE_HYBRID_5TH_GEN: HyundaiCarInfo("Kia Sportage Hybrid 2023", harness_kit=HarnessKit(Harness.hyundai_n)),
+ CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness_kit=HarnessKit(Harness.hyundai_c)),
+ CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022", "All", harness_kit=HarnessKit(Harness.hyundai_k)),
+ CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness_kit=HarnessKit(Harness.hyundai_e)),
CAR.KIA_EV6: [
- HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", harness=Harness.hyundai_p),
- HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", harness=Harness.hyundai_l),
- HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", harness=Harness.hyundai_p)
+ HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_p)),
+ HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", harness_kit=HarnessKit(Harness.hyundai_l)),
+ HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", harness_kit=HarnessKit(Harness.hyundai_p))
],
# Genesis
CAR.GENESIS_GV60_EV_1ST_GEN: [
- HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", harness=Harness.hyundai_a),
- HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", harness=Harness.hyundai_k),
+ HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", harness_kit=HarnessKit(Harness.hyundai_a)),
+ HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", harness_kit=HarnessKit(Harness.hyundai_k)),
],
- CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness=Harness.hyundai_f),
- CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness=Harness.hyundai_f),
+ CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", harness_kit=HarnessKit(Harness.hyundai_f)),
+ CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness_kit=HarnessKit(Harness.hyundai_f)),
CAR.GENESIS_GV70_1ST_GEN: [
- HyundaiCarInfo("Genesis GV70 (2.5T Trim) 2022-23", "All", harness=Harness.hyundai_l),
- HyundaiCarInfo("Genesis GV70 (3.5T Trim) 2022-23", "All", harness=Harness.hyundai_m),
+ HyundaiCarInfo("Genesis GV70 (2.5T Trim) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_l)),
+ HyundaiCarInfo("Genesis GV70 (3.5T Trim) 2022-23", "All", harness_kit=HarnessKit(Harness.hyundai_m)),
],
- CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", harness=Harness.hyundai_h),
- CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness=Harness.hyundai_c),
- CAR.GENESIS_GV80: HyundaiCarInfo("Genesis GV80 2023", "All", harness=Harness.hyundai_m),
+ CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", harness_kit=HarnessKit(Harness.hyundai_h)),
+ CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", harness_kit=HarnessKit(Harness.hyundai_c)),
+ CAR.GENESIS_GV80: HyundaiCarInfo("Genesis GV80 2023", "All", harness_kit=HarnessKit(Harness.hyundai_m)),
}
class Buttons:
@@ -574,6 +574,7 @@ FW_VERSIONS = {
b'\xf1\x82DNCVN5GMCCXXXG2B',
b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_J10',
b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DNDWN5TMDCXXXJ1A',
+ b'\xf1\x8739110-2S041\xf1\x81HM6M1_0a0_M10',
b'\xf1\x87391162M003',
b'\xf1\x87391162M013',
b'\xf1\x87391162M023',
@@ -587,6 +588,7 @@ FW_VERSIONS = {
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware
+ b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC102',
b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', # modified firmware
b'\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101',
b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101',
@@ -624,6 +626,7 @@ FW_VERSIONS = {
b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92',
b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB2\x00\x00\x00\x00\x00\x00\x08\xc9O:',
b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB4\x00\x00\x00\x00\x00\x00g!l[',
+ b'\xf1\x00HT6WA280BLHT6WAE10A1SDN8G25NB5\x00\x00\x00\x00\x00\x00\xe0t\xa9\xba',
b'\xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5',
b'\xf1\x00T02601BL T02832A1 VDN8T25XXX832NS8G\x0e\xfeE',
b'\xf1\x00T02601BL T02900A1 VDN8T25XXX900NSCF\xe4!Y',
@@ -1139,7 +1142,10 @@ FW_VERSIONS = {
],
},
CAR.GENESIS_G90: {
- (Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDGMD15866192DD3x\x88x\x89wuFvvfUf\x88vWwgwwwvfVgx\x87o\xff\xbc^\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7'],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x87VDGMD15352242DD3w\x87gxwvgv\x87wvw\x88wXwffVfffUfw\x88o\xff\x06J\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7',
+ b'\xf1\x87VDGMD15866192DD3x\x88x\x89wuFvvfUf\x88vWwgwwwvfVgx\x87o\xff\xbc^\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7',
+ ],
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00HI__ SCC F-CUP 1.00 1.01 96400-D2100 '],
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00HI LKAS AT USA LHD 1.00 1.00 95895-D2020 160302'],
(Ecu.engine, 0x7e0, None): [b'\xf1\x810000000000\x00'],
diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py
index e2ee93e02..3b29a2562 100644
--- a/selfdrive/car/mazda/mazdacan.py
+++ b/selfdrive/car/mazda/mazdacan.py
@@ -1,5 +1,3 @@
-import copy
-
from selfdrive.car.mazda.values import GEN1, Buttons
@@ -64,7 +62,17 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool):
- values = copy.copy(cam_msg)
+ values = {s: cam_msg[s] for s in [
+ "LINE_VISIBLE",
+ "LINE_NOT_VISIBLE",
+ "LANE_LINES",
+ "BIT1",
+ "BIT2",
+ "BIT3",
+ "NO_ERR_BIT",
+ "S1",
+ "S1_HBEAM",
+ ]}
values.update({
# TODO: what's the difference between all these? do we need to send all?
"HANDS_WARN_3_BITS": 0b111 if steer_required else 0,
diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py
index 8f993e265..79f07a1fd 100644
--- a/selfdrive/car/mazda/values.py
+++ b/selfdrive/car/mazda/values.py
@@ -1,10 +1,9 @@
from dataclasses import dataclass
-from enum import Enum
from typing import Dict, List, Union
from cereal import car
from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarInfo, Harness
+from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
@@ -38,7 +37,7 @@ class CAR:
@dataclass
class MazdaCarInfo(CarInfo):
package: str = "All"
- harness: Enum = Harness.mazda
+ harness_kit: HarnessKit = HarnessKit(Harness.mazda)
CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = {
diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py
index 73493a974..5064e8c45 100644
--- a/selfdrive/car/nissan/carcontroller.py
+++ b/selfdrive/car/nissan/carcontroller.py
@@ -61,7 +61,7 @@ class CarController:
can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd))
can_sends.append(nissancan.create_steering_control(
- self.packer, apply_angle, self.frame, CC.enabled, self.lkas_max_torque))
+ self.packer, apply_angle, self.frame, CC.latActive, self.lkas_max_torque))
if self.CP.carFingerprint != CAR.ALTIMA:
if self.frame % 2 == 0:
diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py
index d0f0aa93c..cbe62645d 100644
--- a/selfdrive/car/nissan/values.py
+++ b/selfdrive/car/nissan/values.py
@@ -1,11 +1,10 @@
from dataclasses import dataclass
from typing import Dict, List, Optional, Union
-from enum import Enum
from cereal import car
from panda.python import uds
from selfdrive.car import AngleRateLimit, dbc_dict
-from selfdrive.car.docs_definitions import CarInfo, Harness
+from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit, HarnessPart
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
@@ -31,10 +30,13 @@ class CAR:
ALTIMA = "NISSAN ALTIMA 2020"
+NISSAN_HARNESS_PARTS = [HarnessPart.harness_box, HarnessPart.rj45_cable, HarnessPart.long_obdc_cable, HarnessPart.usbc_coupler]
+
+
@dataclass
class NissanCarInfo(CarInfo):
package: str = "ProPILOT Assist"
- harness: Enum = Harness.nissan_a
+ harness_kit: HarnessKit = HarnessKit(Harness.nissan_a, parts=NISSAN_HARNESS_PARTS)
CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = {
@@ -42,7 +44,7 @@ CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = {
CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-23", video_link="https://youtu.be/vaMbtAh_0cY"),
CAR.LEAF_IC: None, # same platforms
CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"),
- CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness=Harness.nissan_b),
+ CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness_kit=HarnessKit(Harness.nissan_b, parts=NISSAN_HARNESS_PARTS)),
}
FINGERPRINTS = {
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index 8cd3239a0..f7698dbe7 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -21,7 +21,7 @@ class CarInterface(CarInterfaceBase):
if candidate in PREGLOBAL_CARS:
ret.enableBsm = 0x25c in fingerprint[0]
- ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruLegacy)]
+ ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruPreglobal)]
else:
ret.enableBsm = 0x228 in fingerprint[0]
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)]
diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py
index 446f9be2a..1977b43f9 100644
--- a/selfdrive/car/subaru/values.py
+++ b/selfdrive/car/subaru/values.py
@@ -1,11 +1,11 @@
from dataclasses import dataclass
-from enum import Enum, IntFlag
+from enum import IntFlag
from typing import Dict, List, Union
from cereal import car
from panda.python import uds
from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarInfo, Harness
+from selfdrive.car.docs_definitions import CarInfo, Harness, HarnessKit
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
Ecu = car.CarParams.Ecu
@@ -53,13 +53,13 @@ class CAR:
@dataclass
class SubaruCarInfo(CarInfo):
package: str = "EyeSight Driver Assistance"
- harness: Enum = Harness.subaru_a
+ harness_kit: HarnessKit = HarnessKit(Harness.subaru_a)
CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = {
CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-21", "All"),
- CAR.OUTBACK: SubaruCarInfo("Subaru Outback 2020-22", "All", harness=Harness.subaru_b),
- CAR.LEGACY: SubaruCarInfo("Subaru Legacy 2020-22", "All", harness=Harness.subaru_b),
+ CAR.OUTBACK: SubaruCarInfo("Subaru Outback 2020-22", "All", harness_kit=HarnessKit(Harness.subaru_b)),
+ CAR.LEGACY: SubaruCarInfo("Subaru Legacy 2020-22", "All", harness_kit=HarnessKit(Harness.subaru_b)),
CAR.IMPREZA: [
SubaruCarInfo("Subaru Impreza 2017-19"),
SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"),
@@ -510,6 +510,7 @@ FW_VERSIONS = {
b'\xf1\x82\xe2,\xa0@\x07',
b'\xbc"`q\x07',
b'\xe3,\xa0@\x07',
+ b'\xbc,\xa0u\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\xa5\xfe\xf7@\x00',
@@ -519,6 +520,7 @@ FW_VERSIONS = {
b'\xf1\x82\xa7\xf6D@\x00',
b'\xa7\xf6D@\x00',
b'\xa7\xfe\xf4@\x00',
+ b'\xa5\xfe\xf8@\x00',
],
},
}
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index ab585f496..00552485a 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -6,7 +6,7 @@ from typing import Dict, List, Union
from cereal import car
from common.conversions import Conversions as CV
from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness
+from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
@@ -102,7 +102,7 @@ class Footnote(Enum):
@dataclass
class ToyotaCarInfo(CarInfo):
package: str = "All"
- harness: Enum = Harness.toyota
+ harness_kit: HarnessKit = HarnessKit(Harness.toyota)
CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
index b2919e0ac..8ae0ec841 100644
--- a/selfdrive/car/volkswagen/carcontroller.py
+++ b/selfdrive/car/volkswagen/carcontroller.py
@@ -101,8 +101,7 @@ class CarController:
gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last
if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume):
- counter = (CS.gra_stock_values["COUNTER"] + 1) % 16
- can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, counter,
+ can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values,
cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
new_actuators = actuators.copy()
diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py
index b461fd02a..849d99592 100644
--- a/selfdrive/car/volkswagen/mqbcan.py
+++ b/selfdrive/car/volkswagen/mqbcan.py
@@ -11,7 +11,15 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled):
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
- values = ldw_stock_values.copy()
+ values = {}
+ if len(ldw_stock_values):
+ values = {s: ldw_stock_values[s] for s in [
+ "LDW_SW_Warnung_links", # Blind spot in warning mode on left side due to lane departure
+ "LDW_SW_Warnung_rechts", # Blind spot in warning mode on right side due to lane departure
+ "LDW_Seite_DLCTLC", # Direction of most likely lane departure (left or right)
+ "LDW_DLC", # Lane departure, distance to line crossing
+ "LDW_TLC", # Lane departure, time to line crossing
+ ]}
values.update({
"LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0,
@@ -23,11 +31,17 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres
return packer.make_can_msg("LDW_02", bus, values)
-def create_acc_buttons_control(packer, bus, gra_stock_values, counter, cancel=False, resume=False):
- values = gra_stock_values.copy()
+def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False):
+ values = {s: gra_stock_values[s] for s in [
+ "GRA_Hauptschalter", # ACC button, on/off
+ "GRA_Typ_Hauptschalter", # ACC main button type
+ "GRA_Codierung", # ACC button configuration/coding
+ "GRA_Tip_Stufe_2", # unknown related to stalk type
+ "GRA_ButtonTypeInfo", # unknown related to stalk type
+ ]}
values.update({
- "COUNTER": counter,
+ "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
"GRA_Abbrechen": cancel,
"GRA_Tip_Wiederaufnahme": resume,
})
diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py
index bac3ca121..f42c3cf78 100644
--- a/selfdrive/car/volkswagen/pqcan.py
+++ b/selfdrive/car/volkswagen/pqcan.py
@@ -10,7 +10,15 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled):
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
- values = ldw_stock_values.copy()
+ values = {}
+ if len(ldw_stock_values):
+ values = {s: ldw_stock_values[s] for s in [
+ "LDW_SW_Warnung_links", # Blind spot in warning mode on left side due to lane departure
+ "LDW_SW_Warnung_rechts", # Blind spot in warning mode on right side due to lane departure
+ "LDW_Seite_DLCTLC", # Direction of most likely lane departure (left or right)
+ "LDW_DLC", # Lane departure, distance to line crossing
+ "LDW_TLC", # Lane departure, time to line crossing
+ ]}
values.update({
"LDW_Lampe_gelb": 1 if enabled and steering_pressed else 0,
@@ -23,11 +31,16 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres
return packer.make_can_msg("LDW_Status", bus, values)
-def create_acc_buttons_control(packer, bus, gra_stock_values, counter, cancel=False, resume=False):
- values = gra_stock_values.copy()
+def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False):
+ values = {s: gra_stock_values[s] for s in [
+ "GRA_Hauptschalt", # ACC button, on/off
+ "GRA_Typ_Hauptschalt", # ACC button, momentary vs latching
+ "GRA_Kodierinfo", # ACC button, configuration
+ "GRA_Sender", # ACC button, CAN message originator
+ ]}
values.update({
- "COUNTER": counter,
+ "COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
"GRA_Abbrechen": cancel,
"GRA_Recall": resume,
})
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index 5fd3925a2..e4a57ec23 100755
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -7,7 +7,7 @@ from cereal import car
from panda.python import uds
from opendbc.can.can_define import CANDefine
from selfdrive.car import dbc_dict
-from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness
+from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, HarnessKit, HarnessPart
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16
Ecu = car.CarParams.Ecu
@@ -166,7 +166,7 @@ class Footnote(Enum):
@dataclass
class VWCarInfo(CarInfo):
package: str = "Adaptive Cruise Control (ACC) & Lane Assist"
- harness: Enum = Harness.j533
+ harness_kit: HarnessKit = HarnessKit(Harness.j533, parts=[HarnessPart.harness_box, HarnessPart.long_obdc_cable, HarnessPart.usbc_coupler])
def init_make(self, CP: car.CarParams):
self.footnotes.insert(0, Footnote.VW_EXP_LONG)
@@ -227,7 +227,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
VWCarInfo("Volkswagen Tiguan 2018-23"),
VWCarInfo("Volkswagen Tiguan eHybrid 2021-23"),
],
- CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2017"),
+ CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2016-23"),
CAR.TRANSPORTER_T61: [
VWCarInfo("Volkswagen Caravelle 2020"),
VWCarInfo("Volkswagen California 2021"),
@@ -292,33 +292,35 @@ FW_QUERY_CONFIG = FwQueryConfig(
FW_VERSIONS = {
CAR.ARTEON_MK1: {
(Ecu.engine, 0x7e0, None): [
- b'\xf1\x873G0906259M \xf1\x890003',
b'\xf1\x873G0906259F \xf1\x890004',
+ b'\xf1\x873G0906259G \xf1\x890004',
+ b'\xf1\x873G0906259G \xf1\x890005',
+ b'\xf1\x873G0906259M \xf1\x890003',
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\x870DL300014C \xf1\x893704',
b'\xf1\x8709G927158L \xf1\x893611',
+ b'\xf1\x870DL300014C \xf1\x893704',
b'\xf1\x870GC300011L \xf1\x891401',
b'\xf1\x870GC300014M \xf1\x892802',
b'\xf1\x870GC300040P \xf1\x891401',
],
(Ecu.srs, 0x715, None): [
- b'\xf1\x875QF959655AP\xf1\x890755\xf1\x82\x1311110011111311111100110200--1611125F49',
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',
+ b'\xf1\x873Q0959655DA\xf1\x890720\xf1\x82\x0e1712141712141105121122052900',
+ b'\xf1\x873Q0959655DL\xf1\x890732\xf1\x82\0161812141812171105141123052J00',
+ b'\xf1\x875QF959655AP\xf1\x890755\xf1\x82\x1311110011111311111100110200--1611125F49',
],
(Ecu.eps, 0x712, None): [
- b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x002NB4202N7N',
b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571B41815A1',
b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571B00817A1',
b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567B0020800',
b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x002MB4092M7N',
+ b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x002NB4202N7N',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x872Q0907572AA\xf1\x890396',
@@ -1254,9 +1256,10 @@ FW_VERSIONS = {
b'\xf1\x870D9300012 \xf1\x894940',
b'\xf1\x870D9300013A \xf1\x894905',
b'\xf1\x870D9300041H \xf1\x894905',
- b'\xf1\x870GC300014M \xf1\x892801',
- b'\xf1\x870GC300043 \xf1\x892301',
b'\xf1\x870D9300043F \xf1\x895202',
+ b'\xf1\x870GC300014M \xf1\x892801',
+ b'\xf1\x870GC300019G \xf1\x892803',
+ b'\xf1\x870GC300043 \xf1\x892301',
],
(Ecu.srs, 0x715, None): [
b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\x12111200111121001121110012211292221111',
@@ -1265,6 +1268,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02331310031313100313131013141319331413100',
b'\xf1\x875Q0959655CA\xf1\x890403\xf1\x82\x1331310031313100313151013141319331423100',
b'\xf1\x875Q0959655CH\xf1\x890421\xf1\x82\x1333310031313100313152025350539331463100',
+ b'\xf1\x875Q0959655CH\xf1\x890421\xf1\x82\x1333310031313100313152855372539331463100',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820514UZ070203',
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 90ab96ffe..ac9496bae 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -26,7 +26,6 @@ from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
from selfdrive.controls.lib.vehicle_model import VehicleModel
-from selfdrive.locationd.calibrationd import Calibration
from system.hardware import HARDWARE, TICI
SOFT_DISABLE_TIME = 3 # seconds
@@ -302,9 +301,12 @@ class Controls:
# Handle calibration status
cal_status = self.sm['liveCalibration'].calStatus
- if cal_status != Calibration.CALIBRATED:
- if cal_status == Calibration.UNCALIBRATED:
+ if cal_status != log.LiveCalibrationData.Status.calibrated:
+ if cal_status == log.LiveCalibrationData.Status.uncalibrated:
self.events.add(EventName.calibrationIncomplete)
+ elif cal_status == log.LiveCalibrationData.Status.recalibrating:
+ set_offroad_alert("Offroad_Recalibration", True)
+ self.events.add(EventName.calibrationRecalibrating)
else:
self.events.add(EventName.calibrationInvalid)
@@ -354,7 +356,7 @@ class Controls:
self.events.add(EventName.cameraFrameRate)
if not REPLAY and self.rk.lagging:
self.events.add(EventName.controlsdLagging)
- if len(self.sm['radarState'].radarErrors) or not self.sm.all_checks(['radarState']):
+ if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])):
self.events.add(EventName.radarFault)
if not self.sm.valid['pandaStates']:
self.events.add(EventName.usbError)
@@ -598,7 +600,7 @@ class Controls:
CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
if self.dp_alka and not standstill and CS.cruiseState.available:
- if self.sm['liveCalibration'].calStatus != Calibration.CALIBRATED:
+ if self.sm['liveCalibration'].calStatus != log.LiveCalibrationData.Status.calibrated:
pass
elif CS.steerFaultTemporary or CS.steerFaultPermanent:
pass
@@ -729,7 +731,7 @@ class Controls:
recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
- and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
+ and not CC.latActive and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated
model_v2 = self.sm['modelV2']
desire_prediction = model_v2.meta.desirePrediction
diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json
index 2f85ea917..42d2512e5 100644
--- a/selfdrive/controls/lib/alerts_offroad.json
+++ b/selfdrive/controls/lib/alerts_offroad.json
@@ -48,5 +48,9 @@
"Offroad_NoFirmware": {
"text": "openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai.",
"severity": 0
+ },
+ "Offroad_Recalibration": {
+ "text": "openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.",
+ "severity": 0
}
}
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index c389f6354..fd9836865 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -235,7 +235,7 @@ def startup_master_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
return StartupAlert(_("WARNING: This branch is not tested"), branch, alert_status=AlertStatus.userPrompt)
def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
- return NoEntryAlert(f"Drive above {get_display_speed(CP.minEnableSpeed, metric)} to engage")
+ return NoEntryAlert(_("Drive above {speed} to engage").format(speed=get_display_speed(CP.minEnableSpeed, metric)))
def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
@@ -247,9 +247,10 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
+ first_word = _('Recalibration') if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else _('Calibration')
return Alert(
- _("Calibration in Progress: %d%%") % sm['liveCalibration'].calPerc,
- _("Drive Above %s") % get_display_speed(MIN_SPEED_FILTER, metric),
+ _("{word} in Progress: {perc}%").format(word=first_word, perc=sm['liveCalibration'].calPerc),
+ _("Drive Above {speed}").format(speed=get_display_speed(MIN_SPEED_FILTER, metric)),
AlertStatus.normal, AlertSize.mid,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
@@ -271,7 +272,7 @@ def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMas
def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
mdl = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else math.nan
err = CS.vEgo - mdl
- msg = f"Speed Error: {err:.1f} m/s"
+ msg = _("Speed Error: {err:.1f} m/s").format(err=err)
return NoEntryAlert(msg, alert_text_1=_("Posenet Speed Invalid"))
@@ -297,7 +298,7 @@ def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging
rpy = sm['liveCalibration'].rpyCalib
yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan)
pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan)
- angles = f"Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)"
+ angles = _("Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)").format(pitch=pitch, yaw=yaw)
return NormalPermanentAlert(_("Calibration Invalid"), angles)
@@ -309,7 +310,7 @@ def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster,
def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
- return NormalPermanentAlert(_("Low Memory"), f"{sm['deviceState'].memoryUsagePercent}% used")
+ return NormalPermanentAlert(_("Low Memory"), _("{memory_usage_percent}% used").format(memory_usage_percent=sm['deviceState'].memoryUsagePercent))
def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
@@ -318,7 +319,7 @@ def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
- return NormalPermanentAlert(_("Driving Model Lagging"), f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped")
+ return NormalPermanentAlert(_("Driving Model Lagging"), _("{frame_drop_perc:.1f}% frames dropped").format(frame_drop_perc=sm['modelV2'].frameDropPerc))
def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
@@ -331,7 +332,10 @@ def wrong_car_mode_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubM
def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
axes = sm['testJoystick'].axes
gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
- vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
+ vals = _("Gas: {gas_percent}%, Steer: {steer_percent}%").format(
+ gas_percent=round(gb * 100.),
+ steer_percent=round(steer * 100.)
+ )
return NormalPermanentAlert(_("Joystick Mode"), vals)
@@ -725,10 +729,16 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
EventName.calibrationIncomplete: {
ET.PERMANENT: calibration_incomplete_alert,
- ET.SOFT_DISABLE: soft_disable_alert(_("Device remount detected: recalibrating")),
+ ET.SOFT_DISABLE: soft_disable_alert(_("Calibration Incomplete")),
ET.NO_ENTRY: NoEntryAlert(_("Calibration in Progress")),
},
+ EventName.calibrationRecalibrating: {
+ ET.PERMANENT: calibration_incomplete_alert,
+ ET.SOFT_DISABLE: soft_disable_alert(_("Device Remount Detected: Recalibrating")),
+ ET.NO_ENTRY: NoEntryAlert(_("Remount Detected: Recalibrating")),
+ },
+
EventName.doorOpen: {
ET.SOFT_DISABLE: user_soft_disable_alert(_("Door Open")),
ET.NO_ENTRY: NoEntryAlert(_("Door Open")),
@@ -952,9 +962,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.vehicleSensorsInvalid: {
- ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Vehicle Sensors Invalid"),
- ET.PERMANENT: NormalPermanentAlert("Vehicle Sensors Calibrating", "Drive to Calibrate"),
- ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"),
+ ET.IMMEDIATE_DISABLE: ImmediateDisableAlert(_("Vehicle Sensors Invalid")),
+ ET.PERMANENT: NormalPermanentAlert(_("Vehicle Sensors Calibrating"), _("Drive to Calibrate")),
+ ET.NO_ENTRY: NoEntryAlert(_("Vehicle Sensors Calibrating")),
},
}
diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py
index 91dd2e43e..225657d47 100644
--- a/selfdrive/controls/lib/lane_planner.py
+++ b/selfdrive/controls/lib/lane_planner.py
@@ -34,9 +34,9 @@ class LanePlanner:
self.ll_x = np.zeros((TRAJECTORY_SIZE,))
self.lll_y = np.zeros((TRAJECTORY_SIZE,))
self.rll_y = np.zeros((TRAJECTORY_SIZE,))
- self.lane_width_estimate = FirstOrderFilter(3.7, 9.95, DT_MDL)
+ self.lane_width_estimate = FirstOrderFilter(2.7, 9.95, DT_MDL)
self.lane_width_certainty = FirstOrderFilter(1.0, 0.95, DT_MDL)
- self.lane_width = 3.7
+ self.lane_width = 2.7
self.lll_prob = 0.
self.rll_prob = 0.
@@ -93,7 +93,7 @@ class LanePlanner:
self.lane_width_certainty.update(l_prob * r_prob)
current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
self.lane_width_estimate.update(current_lane_width)
- speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
+ speed_lane_width = interp(v_ego, [0., 31.], [2.7, 3.5])
self.lane_width = self.lane_width_certainty.x * self.lane_width_estimate.x + \
(1 - self.lane_width_certainty.x) * speed_lane_width
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json b/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json
new file mode 100644
index 000000000..af9c99f25
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json
@@ -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
+ ]
+ }
+}
\ No newline at end of file
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
new file mode 100644
index 000000000..86b9c84c9
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
@@ -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_
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
new file mode 100644
index 000000000..f8371f3e8
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
@@ -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)
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
index 61e0726fd..7ada7d6c8 100644
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@@ -9,6 +9,8 @@ from selfdrive.controls.lib.desire_helper import DesireHelper
import cereal.messaging as messaging
from cereal import log
from selfdrive.hardware import EON
+from common.params import Params
+from selfdrive.controls.lib.lane_planner import LanePlanner
TRAJECTORY_SIZE = 33
if EON:
@@ -32,6 +34,11 @@ class LateralPlanner:
def __init__(self, CP):
self.DH = DesireHelper()
+ params = Params()
+ self.dp_lat_lane_priority_mode = params.get_bool("dp_lat_lane_priority_mode")
+ self.dp_lat_lane_priority_mode_active = False
+ self.LP = LanePlanner() if self.dp_lat_lane_priority_mode else None
+
# Vehicle model parameters used to calculate lateral movement of car
self.factor1 = CP.wheelbase - CP.centerToFront
self.factor2 = (CP.centerToFront * CP.mass) / (CP.wheelbase * CP.tireStiffnessRear)
@@ -42,7 +49,11 @@ class LateralPlanner:
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE)
- self.y_pts = np.zeros(TRAJECTORY_SIZE)
+ self.y_pts = np.zeros((TRAJECTORY_SIZE,))
+ self.v_plan = np.zeros((TRAJECTORY_SIZE,))
+ self.v_ego = 0.0
+ self.l_lane_change_prob = 0.0
+ self.r_lane_change_prob = 0.0
self.lat_mpc = LateralMpc()
self.reset_mpc(np.zeros(4))
@@ -69,10 +80,20 @@ class LateralPlanner:
if len(desire_state):
self.l_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeLeft]
self.r_lane_change_prob = desire_state[log.LateralPlan.Desire.laneChangeRight]
- lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
+
+ if self.dp_lat_lane_priority_mode:
+ self.LP.parse_model(md)
+ lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
+ else:
+ lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
+
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
- d_path_xyz = self.path_xyz
+ if self.dp_lat_lane_priority_mode:
+ d_path_xyz = self._get_laneless_laneline_d_path_xyz()
+ else:
+ d_path_xyz = self.path_xyz
+
self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
LATERAL_ACCEL_COST, LATERAL_JERK_COST,
STEERING_RATE_COST)
@@ -130,8 +151,30 @@ class LateralPlanner:
lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
lateralPlan.desire = self.DH.desire
- lateralPlan.useLaneLines = False
+ lateralPlan.useLaneLines = self.dp_lat_lane_priority_mode and self.dp_lat_lane_priority_mode_active
lateralPlan.laneChangeState = self.DH.lane_change_state
lateralPlan.laneChangeDirection = self.DH.lane_change_direction
pm.send('lateralPlan', plan_send)
+
+ def _get_laneless_laneline_d_path_xyz(self):
+ if self.dp_lat_lane_priority_mode and self.LP is not None:
+ # Turn off lanes during lane change
+ if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft:
+ self.LP.lll_prob *= self.DH.lane_change_ll_prob
+ self.LP.rll_prob *= self.DH.lane_change_ll_prob
+
+ # decide what mode should we use
+ if (self.LP.lll_prob + self.LP.rll_prob)/2 < 0.3:
+ self.dp_lat_lane_priority_mode_active = False
+ if (self.LP.lll_prob + self.LP.rll_prob)/2 > 0.5:
+ self.dp_lat_lane_priority_mode_active = True
+
+ # use default path if not active
+ if not self.dp_lat_lane_priority_mode_active:
+ return self.path_xyz
+
+ # use lane planner path
+ return self.LP.get_d_path(self.v_ego, self.t_idxs, self.path_xyz)
+ else:
+ return self.path_xyz
diff --git a/selfdrive/controls/lib/legacy_lateral_mpc_lib/acados_ocp_lat.json b/selfdrive/controls/lib/legacy_lateral_mpc_lib/acados_ocp_lat.json
new file mode 100644
index 000000000..40d639ab8
--- /dev/null
+++ b/selfdrive/controls/lib/legacy_lateral_mpc_lib/acados_ocp_lat.json
@@ -0,0 +1,450 @@
+{
+ "acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
+ "acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
+ "code_export_directory": "/data/openpilot/selfdrive/controls/lib/legacy_lateral_mpc_lib/c_generated_code",
+ "constraints": {
+ "C": [],
+ "C_e": [],
+ "D": [],
+ "constr_type": "BGH",
+ "constr_type_e": "BGH",
+ "idxbu": [],
+ "idxbx": [
+ 2,
+ 3
+ ],
+ "idxbx_0": [
+ 0,
+ 1,
+ 2,
+ 3
+ ],
+ "idxbx_e": [],
+ "idxbxe_0": [
+ 0,
+ 1,
+ 2,
+ 3
+ ],
+ "idxsbu": [],
+ "idxsbx": [],
+ "idxsbx_e": [],
+ "idxsg": [],
+ "idxsg_e": [],
+ "idxsh": [],
+ "idxsh_e": [],
+ "idxsphi": [],
+ "idxsphi_e": [],
+ "lbu": [],
+ "lbx": [
+ -1.5707963267948966,
+ -0.8726646259971648
+ ],
+ "lbx_0": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "lbx_e": [],
+ "lg": [],
+ "lg_e": [],
+ "lh": [],
+ "lh_e": [],
+ "lphi": [],
+ "lphi_e": [],
+ "lsbu": [],
+ "lsbx": [],
+ "lsbx_e": [],
+ "lsg": [],
+ "lsg_e": [],
+ "lsh": [],
+ "lsh_e": [],
+ "lsphi": [],
+ "lsphi_e": [],
+ "ubu": [],
+ "ubx": [
+ 1.5707963267948966,
+ 0.8726646259971648
+ ],
+ "ubx_0": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "ubx_e": [],
+ "ug": [],
+ "ug_e": [],
+ "uh": [],
+ "uh_e": [],
+ "uphi": [],
+ "uphi_e": [],
+ "usbu": [],
+ "usbx": [],
+ "usbx_e": [],
+ "usg": [],
+ "usg_e": [],
+ "ush": [],
+ "ush_e": [],
+ "usphi": [],
+ "usphi_e": []
+ },
+ "cost": {
+ "Vu": [
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ]
+ ],
+ "Vu_0": [
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ]
+ ],
+ "Vx": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vx_0": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vx_e": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vz": [],
+ "Vz_0": [],
+ "W": [
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "W_0": [
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "W_e": [
+ [
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Zl": [],
+ "Zl_e": [],
+ "Zu": [],
+ "Zu_e": [],
+ "cost_ext_fun_type": "casadi",
+ "cost_ext_fun_type_0": "casadi",
+ "cost_ext_fun_type_e": "casadi",
+ "cost_type": "NONLINEAR_LS",
+ "cost_type_0": "NONLINEAR_LS",
+ "cost_type_e": "NONLINEAR_LS",
+ "yref": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "yref_0": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "yref_e": [
+ 0.0,
+ 0.0
+ ],
+ "zl": [],
+ "zl_e": [],
+ "zu": [],
+ "zu_e": []
+ },
+ "cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
+ "dims": {
+ "N": 16,
+ "nbu": 0,
+ "nbx": 2,
+ "nbx_0": 4,
+ "nbx_e": 0,
+ "nbxe_0": 4,
+ "ng": 0,
+ "ng_e": 0,
+ "nh": 0,
+ "nh_e": 0,
+ "np": 2,
+ "nphi": 0,
+ "nphi_e": 0,
+ "nr": 0,
+ "nr_e": 0,
+ "ns": 0,
+ "ns_e": 0,
+ "nsbu": 0,
+ "nsbx": 0,
+ "nsbx_e": 0,
+ "nsg": 0,
+ "nsg_e": 0,
+ "nsh": 0,
+ "nsh_e": 0,
+ "nsphi": 0,
+ "nsphi_e": 0,
+ "nu": 1,
+ "nx": 4,
+ "ny": 3,
+ "ny_0": 3,
+ "ny_e": 2,
+ "nz": 0
+ },
+ "model": {
+ "dyn_disc_fun": null,
+ "dyn_disc_fun_jac": null,
+ "dyn_disc_fun_jac_hess": null,
+ "dyn_ext_fun_type": "casadi",
+ "dyn_source_discrete": null,
+ "gnsf": {
+ "nontrivial_f_LO": 1,
+ "purely_linear": 0
+ },
+ "name": "lat"
+ },
+ "parameter_values": [
+ 0.0,
+ 0.0
+ ],
+ "problem_class": "OCP",
+ "simulink_opts": {
+ "inputs": {
+ "cost_W": 0,
+ "cost_W_0": 0,
+ "cost_W_e": 0,
+ "lbu": 1,
+ "lbx": 1,
+ "lbx_0": 1,
+ "lbx_e": 1,
+ "lg": 1,
+ "lh": 1,
+ "parameter_traj": 1,
+ "reset_solver": 0,
+ "u_init": 0,
+ "ubu": 1,
+ "ubx": 1,
+ "ubx_0": 1,
+ "ubx_e": 1,
+ "ug": 1,
+ "uh": 1,
+ "x_init": 0,
+ "y_ref": 1,
+ "y_ref_0": 1,
+ "y_ref_e": 1
+ },
+ "outputs": {
+ "CPU_time": 1,
+ "CPU_time_lin": 0,
+ "CPU_time_qp": 0,
+ "CPU_time_sim": 0,
+ "KKT_residual": 1,
+ "solver_status": 1,
+ "sqp_iter": 1,
+ "u0": 1,
+ "utraj": 0,
+ "x1": 1,
+ "xtraj": 0
+ },
+ "samplingtime": "t0"
+ },
+ "solver_options": {
+ "Tsim": 0.009765625,
+ "alpha_min": 0.05,
+ "alpha_reduction": 0.7,
+ "collocation_type": "GAUSS_LEGENDRE",
+ "eps_sufficient_descent": 0.0001,
+ "exact_hess_constr": 1,
+ "exact_hess_cost": 1,
+ "exact_hess_dyn": 1,
+ "ext_cost_num_hess": 0,
+ "full_step_dual": 0,
+ "globalization": "FIXED_STEP",
+ "globalization_use_SOC": 0,
+ "hessian_approx": "GAUSS_NEWTON",
+ "hpipm_mode": "BALANCE",
+ "initialize_t_slacks": 0,
+ "integrator_type": "ERK",
+ "levenberg_marquardt": 0.0,
+ "line_search_use_sufficient_descent": 0,
+ "model_external_shared_lib_dir": null,
+ "model_external_shared_lib_name": null,
+ "nlp_solver_max_iter": 100,
+ "nlp_solver_step_length": 1.0,
+ "nlp_solver_tol_comp": 1e-06,
+ "nlp_solver_tol_eq": 1e-06,
+ "nlp_solver_tol_ineq": 1e-06,
+ "nlp_solver_tol_stat": 1e-06,
+ "nlp_solver_type": "SQP_RTI",
+ "print_level": 0,
+ "qp_solver": "PARTIAL_CONDENSING_HPIPM",
+ "qp_solver_cond_N": 1,
+ "qp_solver_iter_max": 1,
+ "qp_solver_tol_comp": null,
+ "qp_solver_tol_eq": null,
+ "qp_solver_tol_ineq": null,
+ "qp_solver_tol_stat": null,
+ "qp_solver_warm_start": 0,
+ "regularize_method": null,
+ "sim_method_jac_reuse": [
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0
+ ],
+ "sim_method_newton_iter": 3,
+ "sim_method_num_stages": [
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4
+ ],
+ "sim_method_num_steps": [
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1
+ ],
+ "tf": 2.5,
+ "time_steps": [
+ 0.009765625,
+ 0.029296875,
+ 0.048828125,
+ 0.068359375,
+ 0.087890625,
+ 0.107421875,
+ 0.126953125,
+ 0.146484375,
+ 0.166015625,
+ 0.185546875,
+ 0.205078125,
+ 0.224609375,
+ 0.244140625,
+ 0.263671875,
+ 0.283203125,
+ 0.302734375
+ ]
+ }
+}
\ No newline at end of file
diff --git a/selfdrive/controls/lib/legacy_lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h b/selfdrive/controls/lib/legacy_lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
new file mode 100644
index 000000000..86b9c84c9
--- /dev/null
+++ b/selfdrive/controls/lib/legacy_lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
@@ -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_
diff --git a/selfdrive/controls/lib/legacy_lateral_mpc_lib/c_generated_code/make_sfun_lat.m b/selfdrive/controls/lib/legacy_lateral_mpc_lib/c_generated_code/make_sfun_lat.m
new file mode 100644
index 000000000..2c6679ecb
--- /dev/null
+++ b/selfdrive/controls/lib/legacy_lateral_mpc_lib/c_generated_code/make_sfun_lat.m
@@ -0,0 +1,125 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+SOURCES = { ...
+ 'lat_model/lat_expl_ode_fun.c', ...
+ 'lat_model/lat_expl_vde_forw.c',...
+ 'lat_cost/lat_cost_y_0_fun.c',...
+ 'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
+ 'lat_cost/lat_cost_y_0_hess.c',...
+ 'lat_cost/lat_cost_y_fun.c',...
+ 'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
+ 'lat_cost/lat_cost_y_hess.c',...
+ 'lat_cost/lat_cost_y_e_fun.c',...
+ 'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
+ 'lat_cost/lat_cost_y_e_hess.c',...
+ 'acados_solver_sfunction_lat.c', ...
+ 'acados_solver_lat.c'
+ };
+
+INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
+
+INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
+ ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
+ ['-I', fullfile(INC_PATH, 'acados')], ...
+ ['-I', fullfile(INC_PATH)]};
+
+
+
+CFLAGS = 'CFLAGS=$CFLAGS';
+LDFLAGS = 'LDFLAGS=$LDFLAGS';
+COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
+COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
+
+
+
+LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
+
+LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
+
+% acados linking libraries and flags
+
+
+mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
+ LIB_PATH, LIBS{:}, SOURCES{:}, ...
+ '-output', 'acados_solver_sfunction_lat' );
+
+fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
+ eval('mexext')] );
+
+
+%% print note on usage of s-function
+fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
+input_note = 'Inputs are:\n';
+i_in = 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
+ ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
+ ' size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
+ ' size [34]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
+ ' size [45]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
+i_in = i_in + 1;
+
+fprintf(input_note)
+
+disp(' ')
+
+output_note = 'Outputs are:\n';
+i_out = 0;
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
+
+fprintf(output_note)
diff --git a/selfdrive/controls/lib/legacy_longitudinal_mpc_lib/acados_ocp_long.json b/selfdrive/controls/lib/legacy_longitudinal_mpc_lib/acados_ocp_long.json
new file mode 100644
index 000000000..d1e7a2f63
--- /dev/null
+++ b/selfdrive/controls/lib/legacy_longitudinal_mpc_lib/acados_ocp_long.json
@@ -0,0 +1,625 @@
+{
+ "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/legacy_longitudinal_mpc_lib/c_generated_code",
+ "constraints": {
+ "C": [],
+ "C_e": [],
+ "D": [],
+ "constr_type": "BGH",
+ "constr_type_e": "BGH",
+ "idxbu": [],
+ "idxbx": [],
+ "idxbx_0": [
+ 0,
+ 1,
+ 2
+ ],
+ "idxbx_e": [],
+ "idxbxe_0": [
+ 0,
+ 1,
+ 2
+ ],
+ "idxsbu": [],
+ "idxsbx": [],
+ "idxsbx_e": [],
+ "idxsg": [],
+ "idxsg_e": [],
+ "idxsh": [
+ 0,
+ 1,
+ 2,
+ 3
+ ],
+ "idxsh_e": [],
+ "idxsphi": [],
+ "idxsphi_e": [],
+ "lbu": [],
+ "lbx": [],
+ "lbx_0": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "lbx_e": [],
+ "lg": [],
+ "lg_e": [],
+ "lh": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "lh_e": [],
+ "lphi": [],
+ "lphi_e": [],
+ "lsbu": [],
+ "lsbx": [],
+ "lsbx_e": [],
+ "lsg": [],
+ "lsg_e": [],
+ "lsh": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "lsh_e": [],
+ "lsphi": [],
+ "lsphi_e": [],
+ "ubu": [],
+ "ubx": [],
+ "ubx_0": [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "ubx_e": [],
+ "ug": [],
+ "ug_e": [],
+ "uh": [
+ 10000.0,
+ 10000.0,
+ 10000.0,
+ 10000.0
+ ],
+ "uh_e": [],
+ "uphi": [],
+ "uphi_e": [],
+ "usbu": [],
+ "usbx": [],
+ "usbx_e": [],
+ "usg": [],
+ "usg_e": [],
+ "ush": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "ush_e": [],
+ "usphi": [],
+ "usphi_e": []
+ },
+ "cost": {
+ "Vu": [
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ]
+ ],
+ "Vu_0": [
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ],
+ [
+ 0.0
+ ]
+ ],
+ "Vx": [
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vx_0": [
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vx_e": [
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Vz": [],
+ "Vz_0": [],
+ "W": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "W_0": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "W_e": [
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ]
+ ],
+ "Zl": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "Zl_e": [],
+ "Zu": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "Zu_e": [],
+ "cost_ext_fun_type": "casadi",
+ "cost_ext_fun_type_0": "casadi",
+ "cost_ext_fun_type_e": "casadi",
+ "cost_type": "NONLINEAR_LS",
+ "cost_type_0": "NONLINEAR_LS",
+ "cost_type_e": "NONLINEAR_LS",
+ "yref": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "yref_0": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "yref_e": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "zl": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "zl_e": [],
+ "zu": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "zu_e": []
+ },
+ "cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
+ "dims": {
+ "N": 12,
+ "nbu": 0,
+ "nbx": 0,
+ "nbx_0": 3,
+ "nbx_e": 0,
+ "nbxe_0": 3,
+ "ng": 0,
+ "ng_e": 0,
+ "nh": 4,
+ "nh_e": 0,
+ "np": 4,
+ "nphi": 0,
+ "nphi_e": 0,
+ "nr": 0,
+ "nr_e": 0,
+ "ns": 4,
+ "ns_e": 0,
+ "nsbu": 0,
+ "nsbx": 0,
+ "nsbx_e": 0,
+ "nsg": 0,
+ "nsg_e": 0,
+ "nsh": 4,
+ "nsh_e": 0,
+ "nsphi": 0,
+ "nsphi_e": 0,
+ "nu": 1,
+ "nx": 3,
+ "ny": 6,
+ "ny_0": 6,
+ "ny_e": 5,
+ "nz": 0
+ },
+ "model": {
+ "dyn_disc_fun": null,
+ "dyn_disc_fun_jac": null,
+ "dyn_disc_fun_jac_hess": null,
+ "dyn_ext_fun_type": "casadi",
+ "dyn_source_discrete": null,
+ "gnsf": {
+ "nontrivial_f_LO": 1,
+ "purely_linear": 0
+ },
+ "name": "long"
+ },
+ "parameter_values": [
+ -1.2,
+ 1.2,
+ 0.0,
+ 0.0
+ ],
+ "problem_class": "OCP",
+ "simulink_opts": {
+ "inputs": {
+ "cost_W": 0,
+ "cost_W_0": 0,
+ "cost_W_e": 0,
+ "lbu": 1,
+ "lbx": 1,
+ "lbx_0": 1,
+ "lbx_e": 1,
+ "lg": 1,
+ "lh": 1,
+ "parameter_traj": 1,
+ "reset_solver": 0,
+ "u_init": 0,
+ "ubu": 1,
+ "ubx": 1,
+ "ubx_0": 1,
+ "ubx_e": 1,
+ "ug": 1,
+ "uh": 1,
+ "x_init": 0,
+ "y_ref": 1,
+ "y_ref_0": 1,
+ "y_ref_e": 1
+ },
+ "outputs": {
+ "CPU_time": 1,
+ "CPU_time_lin": 0,
+ "CPU_time_qp": 0,
+ "CPU_time_sim": 0,
+ "KKT_residual": 1,
+ "solver_status": 1,
+ "sqp_iter": 1,
+ "u0": 1,
+ "utraj": 0,
+ "x1": 1,
+ "xtraj": 0
+ },
+ "samplingtime": "t0"
+ },
+ "solver_options": {
+ "Tsim": 0.06944444444444445,
+ "alpha_min": 0.05,
+ "alpha_reduction": 0.7,
+ "collocation_type": "GAUSS_LEGENDRE",
+ "eps_sufficient_descent": 0.0001,
+ "exact_hess_constr": 1,
+ "exact_hess_cost": 1,
+ "exact_hess_dyn": 1,
+ "ext_cost_num_hess": 0,
+ "full_step_dual": 0,
+ "globalization": "FIXED_STEP",
+ "globalization_use_SOC": 0,
+ "hessian_approx": "GAUSS_NEWTON",
+ "hpipm_mode": "BALANCE",
+ "initialize_t_slacks": 0,
+ "integrator_type": "ERK",
+ "levenberg_marquardt": 0.0,
+ "line_search_use_sufficient_descent": 0,
+ "model_external_shared_lib_dir": null,
+ "model_external_shared_lib_name": null,
+ "nlp_solver_max_iter": 100,
+ "nlp_solver_step_length": 1.0,
+ "nlp_solver_tol_comp": 1e-06,
+ "nlp_solver_tol_eq": 1e-06,
+ "nlp_solver_tol_ineq": 1e-06,
+ "nlp_solver_tol_stat": 1e-06,
+ "nlp_solver_type": "SQP_RTI",
+ "print_level": 0,
+ "qp_solver": "PARTIAL_CONDENSING_HPIPM",
+ "qp_solver_cond_N": 1,
+ "qp_solver_iter_max": 10,
+ "qp_solver_tol_comp": 0.001,
+ "qp_solver_tol_eq": 0.001,
+ "qp_solver_tol_ineq": 0.001,
+ "qp_solver_tol_stat": 0.001,
+ "qp_solver_warm_start": 0,
+ "regularize_method": null,
+ "sim_method_jac_reuse": [
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0
+ ],
+ "sim_method_newton_iter": 3,
+ "sim_method_num_stages": [
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4,
+ 4
+ ],
+ "sim_method_num_steps": [
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1,
+ 1
+ ],
+ "tf": 10.0,
+ "time_steps": [
+ 0.06944444444444445,
+ 0.20833333333333334,
+ 0.3472222222222222,
+ 0.48611111111111116,
+ 0.6250000000000002,
+ 0.7638888888888886,
+ 0.9027777777777786,
+ 1.041666666666666,
+ 1.1805555555555554,
+ 1.3194444444444455,
+ 1.4583333333333313,
+ 1.5972222222222232
+ ]
+ }
+}
\ No newline at end of file
diff --git a/selfdrive/controls/lib/legacy_longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.h b/selfdrive/controls/lib/legacy_longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.h
new file mode 100644
index 000000000..ee7a74b61
--- /dev/null
+++ b/selfdrive/controls/lib/legacy_longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef ACADOS_SIM_long_H_
+#define ACADOS_SIM_long_H_
+
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#define LONG_NX 3
+#define LONG_NZ 0
+#define LONG_NU 1
+#define LONG_NP 4
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct sim_solver_capsule
+{
+ // acados objects
+ sim_in *acados_sim_in;
+ sim_out *acados_sim_out;
+ sim_solver *acados_sim_solver;
+ sim_opts *acados_sim_opts;
+ sim_config *acados_sim_config;
+ void *acados_sim_dims;
+
+ /* external functions */
+ // ERK
+ external_function_param_casadi * sim_forw_vde_casadi;
+ external_function_param_casadi * sim_expl_ode_fun_casadi;
+ external_function_param_casadi * sim_expl_ode_hess;
+
+ // IRK
+ external_function_param_casadi * sim_impl_dae_fun;
+ external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
+ external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
+ external_function_param_casadi * sim_impl_dae_hess;
+
+ // GNSF
+ external_function_param_casadi * sim_gnsf_phi_fun;
+ external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
+ external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
+ external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+ external_function_param_casadi * sim_gnsf_get_matrices_fun;
+
+} sim_solver_capsule;
+
+
+ACADOS_SYMBOL_EXPORT int long_acados_sim_create(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int long_acados_sim_solve(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int long_acados_sim_free(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int long_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
+
+ACADOS_SYMBOL_EXPORT sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT void * long_acados_get_sim_dims(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_solver * long_acados_get_sim_solver(sim_solver_capsule *capsule);
+
+
+ACADOS_SYMBOL_EXPORT sim_solver_capsule * long_acados_sim_solver_create_capsule(void);
+ACADOS_SYMBOL_EXPORT int long_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // ACADOS_SIM_long_H_
diff --git a/selfdrive/controls/lib/legacy_longitudinal_mpc_lib/c_generated_code/make_sfun_long.m b/selfdrive/controls/lib/legacy_longitudinal_mpc_lib/c_generated_code/make_sfun_long.m
new file mode 100644
index 000000000..e94b9f94c
--- /dev/null
+++ b/selfdrive/controls/lib/legacy_longitudinal_mpc_lib/c_generated_code/make_sfun_long.m
@@ -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.;
+%
+
+SOURCES = { ...
+ 'long_model/long_expl_ode_fun.c', ...
+ 'long_model/long_expl_vde_forw.c',...
+ 'long_cost/long_cost_y_0_fun.c',...
+ 'long_cost/long_cost_y_0_fun_jac_ut_xt.c',...
+ 'long_cost/long_cost_y_0_hess.c',...
+ 'long_cost/long_cost_y_fun.c',...
+ 'long_cost/long_cost_y_fun_jac_ut_xt.c',...
+ 'long_cost/long_cost_y_hess.c',...
+ 'long_cost/long_cost_y_e_fun.c',...
+ 'long_cost/long_cost_y_e_fun_jac_ut_xt.c',...
+ 'long_cost/long_cost_y_e_hess.c',...
+ 'long_constraints/long_constr_h_fun.c', ...
+ 'long_constraints/long_constr_h_fun_jac_uxt_zt_hess.c', ...
+ 'long_constraints/long_constr_h_fun_jac_uxt_zt.c', ...
+ 'acados_solver_sfunction_long.c', ...
+ 'acados_solver_long.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_long' );
+
+fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_long', '.', ...
+ 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 [3]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
+ ' size [3]\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 [52]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [6]\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 [66]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [5]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') lh, size [4]\n ');
+i_in = i_in + 1;
+input_note = strcat(input_note, num2str(i_in), ') uh, size [4]\n ');
+i_in = i_in + 1;
+
+fprintf(input_note)
+
+disp(' ')
+
+output_note = 'Outputs are:\n';
+i_out = 0;
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
+
+fprintf(output_note)
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 0ca7d1b46..46de878af 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -31,9 +31,9 @@ _DP_E2E_LEAD_COUNT = 5
_DP_E2E_STOP_BP = [0., 10., 20., 30., 40., 50., 55.]
_DP_E2E_STOP_DIST = [10, 30., 50., 70., 80., 90., 120.]
-_DP_E2E_STOP_COUNT = 5
+_DP_E2E_STOP_COUNT = 3
-_DP_E2E_SNG_COUNT = 5
+_DP_E2E_SNG_COUNT = 3
_DP_E2E_SNG_ACC_COUNT = 5
_DP_E2E_SWAP_COUNT = 10
@@ -109,6 +109,8 @@ class LongitudinalPlanner:
return reset_state
def conditional_e2e(self, sm):
+ if not sm['controlsState'].experimentalMode:
+ return self._set_dp_e2e_mode('acc', True)
v_ego_kph = sm['carState'].vEgo * 3.6
standstill = sm['carState'].standstill
@@ -145,7 +147,7 @@ class LongitudinalPlanner:
# when we see a lead
# if sm['dragonConf'].dpE2EConditionalVoacc and self.dp_e2e_has_lead:
- if True and self.dp_e2e_has_lead:
+ if self.CP.radarUnavailable and self.dp_e2e_has_lead:
# drive above conditional speed and lead is too close
if lead_dist <= v_ego_kph * self.dp_e2e_tf * interp(v_ego_kph, [50., 60., 80., 85, 90.], [1.25, 1.20, 1.10, 1.05, 1.]) / 3.6:
self.dp_e2e_tf_count += 1
@@ -274,6 +276,7 @@ class LongitudinalPlanner:
longitudinalPlan.hasLead = sm['radarState'].leadOne.status
longitudinalPlan.longitudinalPlanSource = self.mpc.source
longitudinalPlan.fcw = self.fcw
+ longitudinalPlan.longitudinalValid = self.mpc.mode == 'acc'
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
diff --git a/selfdrive/dragonpilot/mapd.py b/selfdrive/dragonpilot/mapd.py
new file mode 100644
index 000000000..d88ff637b
--- /dev/null
+++ b/selfdrive/dragonpilot/mapd.py
@@ -0,0 +1,231 @@
+#!/usr/bin/env python3
+#pylint: skip-file
+# The MIT License
+#
+# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+import cereal.messaging as messaging
+from common.realtime import Ratekeeper, set_core_affinity, set_realtime_priority, sec_since_boot
+from common.params import Params
+import json
+from cereal import log
+import overpy
+import os
+import subprocess
+
+OSM_QUERY = ["/data/media/0/osm/bin/osm3s_query", "--db-dir=/data/media/0/osm/db/"]
+
+OSM_ONLINE_QUERY_THRESHOLD = 5 # secs
+OSM_LOCAL_QUERY_THRESHOLD = 3 # times
+
+class OSM():
+ def __init__(self, last_gps_pos):
+ self._api = overpy.Overpass()
+
+ self._local_osm_query_fail_count = 0
+ self._last_gps_pos = last_gps_pos
+ self._fetch_time_prev = 0.
+ self._way_tags = []
+
+ self.way_id = 0
+ self.road_name = None
+ self.speed_limit = 0
+
+ if os.path.isdir("/data/media/0/osm/bin/") and os.path.isdir("/data/media/0/osm/db/"):
+ self.local_osm_enabled = True
+ print("Local OSM installed")
+ else:
+ self.local_osm_enabled = False
+ print("Local OSM enabled = %s" % self.local_osm_enabled)
+
+ def process_res(self, res):
+ if len(res) > 0:
+ self._way_tags = res[0].tags
+ self.way_id = res[0].id
+
+
+ def fetch_tags_around_location(self, latitude, longitude):
+ if len(self._way_tags) > 0 and len(self._last_gps_pos) > 0 and self._last_gps_pos["latitude"] == latitude and self._last_gps_pos["longitude"] == longitude:
+ return self._way_tags
+ q = f"""
+ way(around:10, {latitude}, {longitude})
+ [highway]
+ [highway~"^(motorway|trunk|primary|secondary|tertiary|unclassified|residential)$"];
+ (
+ way._["name"];
+ way._["maxspeed"];
+ (._;>;);
+ );
+ out tags;
+ """
+ if self.local_osm_enabled:
+ try:
+ completion = subprocess.run(OSM_QUERY + [f"--request={q}"], check=True, capture_output=True)
+ res = self._api.parse_xml(completion.stdout).ways
+ self.process_res(res)
+ except Exception as e:
+ self._local_osm_query_fail_count += 1
+ pass
+
+ # use remote OSM when local osm is not enabled or failed too many times
+ if not self.local_osm_enabled or self._local_osm_query_fail_count >= OSM_LOCAL_QUERY_THRESHOLD:
+ fetch_time = sec_since_boot()
+ if fetch_time - self._fetch_time_prev < OSM_ONLINE_QUERY_THRESHOLD:
+ return
+ try:
+ res = self._api.query(q).ways
+ self.process_res(res)
+ self._local_osm_query_fail_count = 0
+ except Exception as e:
+ print(f'Exception while querying OSM:\n{e}')
+ self._fetch_time_prev = fetch_time
+
+ self._process_tags()
+
+ def _process_tags(self):
+ if len(self._way_tags) > 0:
+ for tag in self._way_tags:
+ if tag.startswith("name"):
+ self.road_name = self._way_tags[tag]
+ elif tag.startswith("maxspeed"):
+ self.speed_limit = self._way_tags[tag]
+
+class MapD():
+ def __init__(self, position_service):
+ last_gps_params = Params().get('LastGPSPosition')
+ self.last_gps_pos = json.loads(last_gps_params) if last_gps_params is not None else []
+ self.osm = OSM(self.last_gps_pos)
+
+ self.position_service = position_service
+ self._pause_query = False
+ self._longitude = None
+ self._latitude = None
+
+ self._way_id_prev = 0
+ self._road_name_prev = None
+ self._same_road_count = 0
+
+ def apply_last_gps_pos(self):
+ # use last gps position before first fixed
+ if self.last_gps_pos is not None and len(self.last_gps_pos) > 0:
+ self._latitude = self.last_gps_pos["latitude"]
+ self._longitude = self.last_gps_pos["longitude"]
+ # print("MAPD: Use GPS pos: %s, %s (LastGPSPosition)" % (self._latitude, self._longitude))
+
+ def update_car_state(self, sm):
+ sock = 'carState'
+ if not sm.updated[sock] or not sm.valid[sock]:
+ return
+
+ car_state = sm[sock]
+ self._pause_query = car_state.vEgo <= 2.78 # 10km/h
+
+ def update_position(self, sm):
+ if self.position_service == "liveLocationKalman":
+ self.update_locationd(sm)
+ else:
+ self.update_gps(sm)
+
+ def update_locationd(self, sm):
+ sock = 'liveLocationKalman'
+ if not sm.updated[sock] or not sm.valid[sock]:
+ return
+
+ location = sm[sock]
+ location_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
+
+ if not location_valid:
+ return
+
+ self._latitude = location.positionGeodetic.value[0]
+ self._longitude = location.positionGeodetic.value[1]
+ # print("MAPD: Use GPS pos: %s, %s (locationd)" % (self._latitude, self._longitude))
+
+ def update_gps(self, sm):
+ sock = 'gpsLocationExternal'
+ if not sm.updated[sock] or not sm.valid[sock]:
+ return
+
+ log = sm[sock]
+ self.last_gps = log
+
+ # ignore the message if the fix is invalid
+ if log.flags % 2 == 0:
+ return
+
+ self._latitude = log.latitude
+ self._longitude = log.longitude
+ # print("MAPD: Use GPS pos to: %s, %s (gpsLocationExternal)" % (self._latitude, self._longitude))
+
+ def fetch_osm_data(self):
+ if self._pause_query:
+ return
+ self.osm.fetch_tags_around_location(self._latitude, self._longitude)
+
+ def publish(self, pm):
+ if self._pause_query:
+ return
+ map_data_msg = messaging.new_message('liveMapData')
+ map_data_msg.liveMapData.currentRoadName = "" if self.osm.road_name is None else self.osm.road_name
+
+ if (self.osm.road_name is not None and self.osm.road_name == self._road_name_prev) or self.osm.way_id == self._way_id_prev:
+ self._same_road_count += 1
+ else:
+ self._same_road_count = 0
+ self._road_name_prev = self.osm.road_name
+ self._way_id_prev = self.osm.way_id
+ map_data_msg.liveMapData.speedLimitValid = self._same_road_count > 3 and self.osm.speed_limit > 0
+
+ map_data_msg.liveMapData.speedLimit = self.osm.speed_limit
+
+ pm.send('liveMapData', map_data_msg)
+ # print(f'MAPD *****: Publish: \n{map_data_msg}\n********')
+
+# provides live map data information
+def mapd_thread(sm=None, pm=None):
+ use_locationd = False
+ position_service = "liveLocationKalman" if use_locationd else "gpsLocationExternal"
+
+ set_core_affinity([1,])
+ set_realtime_priority(1)
+ mapd = MapD(position_service)
+ rk = Ratekeeper(0.5, print_delay_threshold=None) # Keeps rate at 0.5 hz
+
+ if sm is None:
+ sm = messaging.SubMaster(["carState", position_service])
+ if pm is None:
+ pm = messaging.PubMaster(['liveMapData'])
+
+ mapd.apply_last_gps_pos()
+ while True:
+ sm.update()
+ mapd.update_car_state(sm)
+ mapd.update_position(sm)
+ mapd.fetch_osm_data()
+ mapd.publish(pm)
+ rk.keep_time()
+
+def main(sm=None, pm=None):
+ mapd_thread(sm, pm)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/selfdrive/legacy_modeld/_dmonitoringmodeld b/selfdrive/legacy_modeld/_dmonitoringmodeld
index 7db8a79f1..721c67575 100755
Binary files a/selfdrive/legacy_modeld/_dmonitoringmodeld and b/selfdrive/legacy_modeld/_dmonitoringmodeld differ
diff --git a/selfdrive/legacy_modeld/_modeld b/selfdrive/legacy_modeld/_modeld
index 48f998826..e03a3b685 100755
Binary files a/selfdrive/legacy_modeld/_modeld and b/selfdrive/legacy_modeld/_modeld differ
diff --git a/selfdrive/legacy_modeld/models/supercombo.thneed b/selfdrive/legacy_modeld/models/supercombo.thneed
index d130d0335..a66c126d3 100644
Binary files a/selfdrive/legacy_modeld/models/supercombo.thneed and b/selfdrive/legacy_modeld/models/supercombo.thneed differ
diff --git a/selfdrive/legacy_monitoring/dmonitoringd.py b/selfdrive/legacy_monitoring/dmonitoringd.py
index 26949035d..a8be95b9b 100755
--- a/selfdrive/legacy_monitoring/dmonitoringd.py
+++ b/selfdrive/legacy_monitoring/dmonitoringd.py
@@ -3,10 +3,10 @@ import gc
import cereal.messaging as messaging
from cereal import car
+from cereal import log
from common.params import Params
from common.realtime import set_realtime_priority
from selfdrive.controls.lib.events import Events
-from selfdrive.locationd.calibrationd import Calibration
from selfdrive.legacy_monitoring.driver_monitor import DriverStatus
@@ -24,7 +24,7 @@ def dmonitoringd_thread(sm=None, pm=None):
driver_status = DriverStatus(rhd=is_rhd)
- sm['liveCalibration'].calStatus = Calibration.INVALID
+ sm['liveCalibration'].calStatus = log.LiveCalibrationData.Status.invalid
sm['liveCalibration'].rpyCalib = [0, 0, 0]
sm['carState'].buttonEvents = []
sm['carState'].standstill = True
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
index 091f21117..dc4ffbb1f 100755
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@@ -39,12 +39,6 @@ YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
DEBUG = os.getenv("DEBUG") is not None
-class Calibration:
- UNCALIBRATED = 0
- CALIBRATED = 1
- INVALID = 2
-
-
def is_calibration_valid(rpy: np.ndarray) -> bool:
return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1]) # type: ignore
@@ -69,6 +63,7 @@ class Calibrator:
rpy_init = RPY_INIT
wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
valid_blocks = 0
+ self.cal_status = log.LiveCalibrationData.Status.uncalibrated
if param_put and calibration_params:
try:
@@ -134,16 +129,20 @@ class Calibrator:
self.calib_spread = np.zeros(3)
if self.valid_blocks < INPUTS_NEEDED:
- self.cal_status = Calibration.UNCALIBRATED
+ if self.cal_status == log.LiveCalibrationData.Status.recalibrating:
+ self.cal_status = log.LiveCalibrationData.Status.recalibrating
+ else:
+ self.cal_status = log.LiveCalibrationData.Status.uncalibrated
elif is_calibration_valid(self.rpy):
- self.cal_status = Calibration.CALIBRATED
+ self.cal_status = log.LiveCalibrationData.Status.calibrated
else:
- self.cal_status = Calibration.INVALID
+ self.cal_status = log.LiveCalibrationData.Status.invalid
# If spread is too high, assume mounting was changed and reset to last block.
# Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
- if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED:
+ if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == log.LiveCalibrationData.Status.calibrated:
self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy)
+ self.cal_status = log.LiveCalibrationData.Status.recalibrating
write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
if self.param_put and write_this_cycle:
@@ -210,7 +209,7 @@ class Calibrator:
if self.not_car:
liveCalibration.validBlocks = INPUTS_NEEDED
- liveCalibration.calStatus = Calibration.CALIBRATED
+ liveCalibration.calStatus = log.LiveCalibrationData.Status.calibrated
liveCalibration.calPerc = 100.
liveCalibration.rpyCalib = [0, 0, 0]
liveCalibration.rpyCalibSpread = self.calib_spread.tolist()
diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py
index e9e2e06b4..71b81cc30 100755
--- a/selfdrive/locationd/laikad.py
+++ b/selfdrive/locationd/laikad.py
@@ -176,17 +176,32 @@ class Laikad:
return position_estimate, position_std, velocity_estimate, velocity_std
+ def gps_time_from_qcom_report(self, gnss_msg):
+ report = gnss_msg.drMeasurementReport
+ if report.source == log.QcomGnss.MeasurementSource.gps:
+ report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
+ elif report.source == log.QcomGnss.MeasurementSource.sbas:
+ report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
+ elif report.source == log.QcomGnss.MeasurementSource.glonass:
+ report_time = GPSTime.from_glonass(report.glonassYear,
+ report.glonassDay,
+ report.glonassMilliseconds / 1000.0)
+ else:
+ raise NotImplementedError(f'Unknownconstellation {report.source}')
+ return report_time
+
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: Understand and use remaining unknown constellations
try:
- good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
+ good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS]
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
+ # Garbage timestamps with week > 32767 are sometimes sent by module.
+ # This is an issue with gpsTime and GLONASS time.
+ report_time = self.gps_time_from_qcom_report(gnss_msg)
+ good_week = report_time.week < np.iinfo(np.int16).max
return good_constellation and good_week
elif gnss_msg.which() == 'measurementReport' and not self.use_qcom:
return True
@@ -195,17 +210,26 @@ class Laikad:
def read_report(self, gnss_msg):
if self.use_qcom:
+ # QCOM reports are per constellation, should always send 3 reports
report = gnss_msg.drMeasurementReport
- week = report.gpsWeek
- tow = report.gpsMilliseconds / 1000.0
- new_meas = read_raw_qcom(report)
+ report_time = self.gps_time_from_qcom_report(gnss_msg)
+
+ if report_time - self.last_report_time > 0:
+ self.qcom_reports = [report]
+ else:
+ self.qcom_reports.append(report)
+ self.last_report_time = report_time
+
+ new_meas = []
+ if len(self.qcom_reports) == 3:
+ for report in self.qcom_reports:
+ new_meas.extend(read_raw_qcom(report))
+
else:
report = gnss_msg.measurementReport
- week = report.gpsWeek
- tow = report.rcvTow
+ self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow)
new_meas = read_raw_ublox(report)
- self.last_report_time = GPSTime(week, tow)
- return week, tow, new_meas
+ return self.last_report_time, new_meas
def is_ephemeris(self, gnss_msg):
if self.use_qcom:
@@ -279,12 +303,11 @@ class Laikad:
if self.is_ephemeris(gnss_msg):
self.read_ephemeris(gnss_msg)
elif self.is_good_report(gnss_msg):
- week, tow, new_meas = self.read_report(gnss_msg)
- self.gps_week = week
- if week > 0:
- latest_msg_t = GPSTime(week, tow)
+ report_t, new_meas = self.read_report(gnss_msg)
+ self.gps_week = report_t.week
+ if report_t.week > 0:
if self.auto_fetch_navs:
- self.fetch_navs(latest_msg_t, block)
+ self.fetch_navs(report_t, block)
corrected_measurements = self.process_report(new_meas, t)
msg_dict['correctedMeasurements'] = [create_measurement_msg(m) for m in corrected_measurements]
diff --git a/selfdrive/locationd/locationd b/selfdrive/locationd/locationd
index 987d89711..96a51ac51 100755
Binary files a/selfdrive/locationd/locationd and b/selfdrive/locationd/locationd differ
diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h
index b405f9f52..703d79316 100644
--- a/selfdrive/locationd/models/generated/car.h
+++ b/selfdrive/locationd/models/generated/car.h
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_6012251674703603378);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_2552300699951834906);
-void car_H_mod_fun(double *state, double *out_4476663174268174194);
-void car_f_fun(double *state, double dt, double *out_2059632134765740243);
-void car_F_fun(double *state, double dt, double *out_5892218022728076105);
-void car_h_25(double *state, double *unused, double *out_1545506078435773641);
-void car_H_25(double *state, double *unused, double *out_6462530086353255637);
-void car_h_24(double *state, double *unused, double *out_1712967250978073065);
-void car_H_24(double *state, double *unused, double *out_5783181307505134553);
-void car_h_30(double *state, double *unused, double *out_1702692746786902786);
-void car_H_30(double *state, double *unused, double *out_6591869033496495707);
-void car_h_26(double *state, double *unused, double *out_7279276425219937979);
-void car_H_26(double *state, double *unused, double *out_8242710668482239755);
-void car_h_27(double *state, double *unused, double *out_3456167189225072004);
-void car_H_27(double *state, double *unused, double *out_8766632345296920618);
-void car_h_29(double *state, double *unused, double *out_7317850665653573656);
-void car_H_29(double *state, double *unused, double *out_7966749001543079965);
-void car_h_28(double *state, double *unused, double *out_643615085955218621);
-void car_H_28(double *state, double *unused, double *out_8516364800601145400);
-void car_h_31(double *state, double *unused, double *out_1820700140720279530);
-void car_H_31(double *state, double *unused, double *out_6431884124476295209);
+void car_err_fun(double *nom_x, double *delta_x, double *out_1673219004331321340);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_7685300362947227633);
+void car_H_mod_fun(double *state, double *out_7906035710387220062);
+void car_f_fun(double *state, double dt, double *out_4520596976096901018);
+void car_F_fun(double *state, double dt, double *out_3330787842923361570);
+void car_h_25(double *state, double *unused, double *out_2485841073551241221);
+void car_H_25(double *state, double *unused, double *out_5566715376639757475);
+void car_h_24(double *state, double *unused, double *out_9107689886717443190);
+void car_H_24(double *state, double *unused, double *out_3651963511000598916);
+void car_h_30(double *state, double *unused, double *out_2210647011266735332);
+void car_H_30(double *state, double *unused, double *out_1039019046512149277);
+void car_h_26(double *state, double *unused, double *out_3131220224619295231);
+void car_H_26(double *state, double *unused, double *out_1825212057765701251);
+void car_h_27(double *state, double *unused, double *out_8884882590965090367);
+void car_H_27(double *state, double *unused, double *out_3262613117696092494);
+void car_h_29(double *state, double *unused, double *out_5247433062711068017);
+void car_H_29(double *state, double *unused, double *out_1549250390826541461);
+void car_h_28(double *state, double *unused, double *out_88573126699093227);
+void car_H_28(double *state, double *unused, double *out_3533148626242989113);
+void car_h_31(double *state, double *unused, double *out_6010819607712498771);
+void car_H_31(double *state, double *unused, double *out_1199003955532349775);
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
void car_set_mass(double x);
void car_set_rotational_inertia(double x);
diff --git a/selfdrive/locationd/models/generated/gnss.h b/selfdrive/locationd/models/generated/gnss.h
index b2455233d..56fe6ccc5 100644
--- a/selfdrive/locationd/models/generated/gnss.h
+++ b/selfdrive/locationd/models/generated/gnss.h
@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void gnss_err_fun(double *nom_x, double *delta_x, double *out_8330333360624833292);
-void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6322537350686059778);
-void gnss_H_mod_fun(double *state, double *out_6515884925296062016);
-void gnss_f_fun(double *state, double dt, double *out_523365963258079124);
-void gnss_F_fun(double *state, double dt, double *out_5168491338484130631);
-void gnss_h_6(double *state, double *sat_pos, double *out_1139312402143986909);
-void gnss_H_6(double *state, double *sat_pos, double *out_2439869829476496896);
-void gnss_h_20(double *state, double *sat_pos, double *out_6664425544558842711);
-void gnss_H_20(double *state, double *sat_pos, double *out_164586234965804468);
-void gnss_h_7(double *state, double *sat_pos_vel, double *out_8735629225209972255);
-void gnss_H_7(double *state, double *sat_pos_vel, double *out_7961471765576663032);
-void gnss_h_21(double *state, double *sat_pos_vel, double *out_8735629225209972255);
-void gnss_H_21(double *state, double *sat_pos_vel, double *out_7961471765576663032);
+void gnss_err_fun(double *nom_x, double *delta_x, double *out_8821442694437018176);
+void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_2669404392972591154);
+void gnss_H_mod_fun(double *state, double *out_3628383119241527248);
+void gnss_f_fun(double *state, double dt, double *out_2911894030962606432);
+void gnss_F_fun(double *state, double dt, double *out_8657587500259786555);
+void gnss_h_6(double *state, double *sat_pos, double *out_1940478305202382078);
+void gnss_H_6(double *state, double *sat_pos, double *out_5861605845652759501);
+void gnss_h_20(double *state, double *sat_pos, double *out_3198189178613923928);
+void gnss_H_20(double *state, double *sat_pos, double *out_3468662543713286228);
+void gnss_h_7(double *state, double *sat_pos_vel, double *out_2083846631906470419);
+void gnss_H_7(double *state, double *sat_pos_vel, double *out_227112697527214299);
+void gnss_h_21(double *state, double *sat_pos_vel, double *out_2083846631906470419);
+void gnss_H_21(double *state, double *sat_pos_vel, double *out_227112697527214299);
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
\ No newline at end of file
diff --git a/selfdrive/locationd/models/generated/libkf.so b/selfdrive/locationd/models/generated/libkf.so
index bde075c44..29c820dbe 100755
Binary files a/selfdrive/locationd/models/generated/libkf.so and b/selfdrive/locationd/models/generated/libkf.so differ
diff --git a/selfdrive/locationd/models/generated/live.h b/selfdrive/locationd/models/generated/live.h
index ed4fed45d..8e4e64f47 100644
--- a/selfdrive/locationd/models/generated/live.h
+++ b/selfdrive/locationd/models/generated/live.h
@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void live_H(double *in_vec, double *out_2363812074447305841);
-void live_err_fun(double *nom_x, double *delta_x, double *out_6269769043913535493);
-void live_inv_err_fun(double *nom_x, double *true_x, double *out_7372408302410611357);
-void live_H_mod_fun(double *state, double *out_323168030093411574);
-void live_f_fun(double *state, double dt, double *out_8727360583652276978);
-void live_F_fun(double *state, double dt, double *out_56582328146481194);
-void live_h_4(double *state, double *unused, double *out_3495874077871324958);
-void live_H_4(double *state, double *unused, double *out_5675340409063545143);
-void live_h_9(double *state, double *unused, double *out_2329430295379681157);
-void live_H_9(double *state, double *unused, double *out_5916530055693135788);
-void live_h_10(double *state, double *unused, double *out_6002666685057306713);
-void live_H_10(double *state, double *unused, double *out_1457761977046980482);
-void live_h_12(double *state, double *unused, double *out_2504055259638529936);
-void live_H_12(double *state, double *unused, double *out_7751947256614044678);
-void live_h_35(double *state, double *unused, double *out_7742259407295802366);
-void live_H_35(double *state, double *unused, double *out_9042002466436152519);
-void live_h_32(double *state, double *unused, double *out_6946137798333760961);
-void live_H_32(double *state, double *unused, double *out_6155521043587698023);
-void live_h_13(double *state, double *unused, double *out_7588585975073727177);
-void live_H_13(double *state, double *unused, double *out_8230890641426416896);
-void live_h_14(double *state, double *unused, double *out_2329430295379681157);
-void live_H_14(double *state, double *unused, double *out_5916530055693135788);
-void live_h_33(double *state, double *unused, double *out_4608549978778917310);
-void live_H_33(double *state, double *unused, double *out_6254184602634541493);
+void live_H(double *in_vec, double *out_7160508158065063740);
+void live_err_fun(double *nom_x, double *delta_x, double *out_9119248387773333404);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_6697903320192654180);
+void live_H_mod_fun(double *state, double *out_277792456163775303);
+void live_f_fun(double *state, double dt, double *out_4978833357093498698);
+void live_F_fun(double *state, double dt, double *out_6849203160499840969);
+void live_h_4(double *state, double *unused, double *out_416461024327247384);
+void live_H_4(double *state, double *unused, double *out_3921598954962045660);
+void live_h_9(double *state, double *unused, double *out_8227592735492858479);
+void live_H_9(double *state, double *unused, double *out_3365619980302401810);
+void live_h_10(double *state, double *unused, double *out_4296550159527316506);
+void live_H_10(double *state, double *unused, double *out_4607961823255245423);
+void live_h_12(double *state, double *unused, double *out_7070759218890925604);
+void live_H_12(double *state, double *unused, double *out_1097857453069916135);
+void live_h_35(double *state, double *unused, double *out_5090403797667048641);
+void live_H_35(double *state, double *unused, double *out_554936897589438284);
+void live_h_32(double *state, double *unused, double *out_6688851263345634384);
+void live_H_32(double *state, double *unused, double *out_6154347310456520517);
+void live_h_13(double *state, double *unused, double *out_4001286955811606869);
+void live_H_13(double *state, double *unused, double *out_9160093207497218410);
+void live_h_14(double *state, double *unused, double *out_8227592735492858479);
+void live_H_14(double *state, double *unused, double *out_3365619980302401810);
+void live_h_33(double *state, double *unused, double *out_3464094948502741824);
+void live_H_33(double *state, double *unused, double *out_2595620107049419320);
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
\ No newline at end of file
diff --git a/selfdrive/loggerd/bootlog b/selfdrive/loggerd/bootlog
index e15c890a7..bd4fc9cb7 100755
Binary files a/selfdrive/loggerd/bootlog and b/selfdrive/loggerd/bootlog differ
diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd
index 311f3b6c9..c62332667 100755
Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ
diff --git a/selfdrive/manager/helpers.py b/selfdrive/manager/helpers.py
index 983c7cc0b..f8607fffc 100644
--- a/selfdrive/manager/helpers.py
+++ b/selfdrive/manager/helpers.py
@@ -36,3 +36,8 @@ def unblock_stdout() -> None:
# whose low byte is the signal number and whose high byte is the exit status
exit_status = os.wait()[1] >> 8
os._exit(exit_status)
+
+
+def write_onroad_params(started, params):
+ params.put_bool("IsOnroad", started)
+ params.put_bool("IsOffroad", not started)
diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py
index b7dcc27a0..85a9806b9 100755
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -14,7 +14,7 @@ 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 selfdrive.manager.helpers import unblock_stdout
+from selfdrive.manager.helpers import unblock_stdout, write_onroad_params
from selfdrive.manager.process import ensure_running
from selfdrive.manager.process_config import managed_processes
from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID
@@ -45,6 +45,8 @@ def manager_init() -> None:
("dp_no_gps_ctrl", "0"),
("dp_no_fan_ctrl", "0"),
("dp_alka", "1"),
+ ("dp_mapd", "1"),
+ ("dp_lat_lane_priority_mode", "0"),
]
if not PC:
default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))
@@ -138,12 +140,15 @@ def manager_thread() -> None:
ignore.append("pandad")
ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0]
+ if not params.get_bool("dp_mapd") or params.get_bool("dp_no_gps_ctrl"):
+ ignore += ["mapd"]
if params.get_bool("dp_no_gps_ctrl"):
- ignore += ["ubloxd", "gpx_uploader", "mapd", "gpxd"]
+ ignore += ["ubloxd", "gpx_uploader", "gpxd"]
sm = messaging.SubMaster(['deviceState', 'carParams'], poll=['deviceState'])
pm = messaging.PubMaster(['managerState'])
+ write_onroad_params(False, params)
ensure_running(managed_processes.values(), False, params=params, CP=sm['carParams'], not_run=ignore)
started_prev = False
@@ -158,10 +163,9 @@ def manager_thread() -> None:
elif not started and started_prev:
params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
- # initialize and update onroad params, which drives boardd's safety setter thread
- if started != started_prev or sm.frame == 0:
- params.put_bool("IsOnroad", started)
- params.put_bool("IsOffroad", not started)
+ # update onroad params, which drives boardd's safety setter thread
+ if started != started_prev:
+ write_onroad_params(started, params)
started_prev = started
diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py
index 201c52bae..e4075b990 100644
--- a/selfdrive/manager/process_config.py
+++ b/selfdrive/manager/process_config.py
@@ -85,7 +85,7 @@ procs = [
PythonProcess("androidd", "system.hardware.eon.androidd", enabled=EON, offroad=True),
# mapd
- PythonProcess("mapd", "selfdrive.mapd.mapd"),
+ PythonProcess("mapd", "selfdrive.dragonpilot.mapd"),
# gpxd
PythonProcess("gpxd", "selfdrive.dragonpilot.gpxd"),
PythonProcess("gpx_uploader", "selfdrive.dragonpilot.gpx_uploader", offroad=True),
diff --git a/selfdrive/mapd/config.py b/selfdrive/mapd/config.py
deleted file mode 100644
index 815644f74..000000000
--- a/selfdrive/mapd/config.py
+++ /dev/null
@@ -1,7 +0,0 @@
-# Map query config
-
-QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries.
-MIN_DISTANCE_FOR_NEW_QUERY = 1000 # mts. Minimum distance to query area edge before issuing a new query.
-FULL_STOP_MAX_SPEED = 0.1 # m/s Max speed for considering car is stopped.
-LOOK_AHEAD_HORIZON_TIME = 15. # s. Time horizon for look ahead of turn speed sections to provide on liveMapData msg.
-LANE_WIDTH = 2.7 # Lane width estimate. Used for detecting departures from way.
diff --git a/selfdrive/mapd/default_speeds.json b/selfdrive/mapd/default_speeds.json
deleted file mode 100644
index 7126c27fc..000000000
--- a/selfdrive/mapd/default_speeds.json
+++ /dev/null
@@ -1,106 +0,0 @@
-{
- "_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped",
- "AR:urban": "40",
- "AR:urban:primary": "60",
- "AR:urban:secondary": "60",
- "AR:rural": "110",
- "AT:urban": "50",
- "AT:rural": "100",
- "AT:trunk": "100",
- "AT:motorway": "130",
- "BE:urban": "50",
- "BE-VLG:rural": "70",
- "BE-WAL:rural": "90",
- "BE:trunk": "120",
- "BE:motorway": "120",
- "CH:urban[1]": "50",
- "CH:rural": "80",
- "CH:trunk": "100",
- "CH:motorway": "120",
- "CZ:pedestrian_zone": "20",
- "CZ:living_street": "20",
- "CZ:urban": "50",
- "CZ:urban_trunk": "80",
- "CZ:urban_motorway": "80",
- "CZ:rural": "90",
- "CZ:trunk": "110",
- "CZ:motorway": "130",
- "DK:urban": "50",
- "DK:rural": "80",
- "DK:motorway": "130",
- "DE:living_street": "7",
- "DE:residential": "30",
- "DE:urban": "50",
- "DE:rural": "100",
- "DE:trunk": "none",
- "DE:motorway": "none",
- "FI:urban": "50",
- "FI:rural": "80",
- "FI:trunk": "100",
- "FI:motorway": "120",
- "FR:urban": "50",
- "FR:rural": "80",
- "FR:trunk": "110",
- "FR:motorway": "130",
- "GR:urban": "50",
- "GR:rural": "90",
- "GR:trunk": "110",
- "GR:motorway": "130",
- "HU:urban": "50",
- "HU:rural": "90",
- "HU:trunk": "110",
- "HU:motorway": "130",
- "IT:urban": "50",
- "IT:rural": "90",
- "IT:trunk": "110",
- "IT:motorway": "130",
- "JP:national": "60",
- "JP:motorway": "100",
- "LT:living_street": "20",
- "LT:urban": "50",
- "LT:rural": "90",
- "LT:trunk": "120",
- "LT:motorway": "130",
- "PL:living_street": "20",
- "PL:urban": "50",
- "PL:rural": "90",
- "PL:trunk": "100",
- "PL:motorway": "140",
- "RO:urban": "50",
- "RO:rural": "90",
- "RO:trunk": "100",
- "RO:motorway": "130",
- "RU:living_street": "20",
- "RU:urban": "60",
- "RU:rural": "90",
- "RU:motorway": "110",
- "SK:urban": "50",
- "SK:rural": "90",
- "SK:trunk": "90",
- "SK:motorway": "90",
- "SI:urban": "50",
- "SI:rural": "90",
- "SI:trunk": "110",
- "SI:motorway": "130",
- "ES:living_street": "20",
- "ES:urban": "50",
- "ES:rural": "50",
- "ES:trunk": "90",
- "ES:motorway": "120",
- "SE:urban": "50",
- "SE:rural": "70",
- "SE:trunk": "90",
- "SE:motorway": "110",
- "GB:nsl_restricted": "30 mph",
- "GB:nsl_single": "60 mph",
- "GB:nsl_dual": "70 mph",
- "GB:motorway": "70 mph",
- "UA:urban": "50",
- "UA:rural": "90",
- "UA:trunk": "110",
- "UA:motorway": "130",
- "UZ:living_street": "30",
- "UZ:urban": "70",
- "UZ:rural": "100",
- "UZ:motorway": "110"
-}
diff --git a/selfdrive/mapd/default_speeds_generator.py b/selfdrive/mapd/default_speeds_generator.py
deleted file mode 100644
index 888c9e1df..000000000
--- a/selfdrive/mapd/default_speeds_generator.py
+++ /dev/null
@@ -1,240 +0,0 @@
-#!/usr/bin/env python
-import json
-
-DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json"
-
-def main(filename = DEFAULT_OUTPUT_FILENAME):
- countries = []
-
- """
- --------------------------------------------------
- US - United State of America
- --------------------------------------------------
- """
- US = Country("US") # First step, create the country using the ISO 3166 two letter code
- countries.append(US) # Second step, add the country to countries list
-
- """ Default rules """
- # Third step, add some default rules for the country
- # Speed limit rules are based on OpenStreetMaps (OSM) tags.
- # The dictionary {...} defines the tag_name: value
- # if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied.
- # The text at the end is the speed limit (use no unit for km/h)
- # Rules apply in the order in which they are written for each country
- # Rules for specific regions (states) take priority over country rules
- # If you modify existing country rules, you must update all existing states without that rule to use the old rule
- US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to 65 mph
- US.add_rule({"highway": "trunk"}, "55 mph")
- US.add_rule({"highway": "primary"}, "55 mph")
- US.add_rule({"highway": "secondary"}, "45 mph")
- US.add_rule({"highway": "tertiary"}, "35 mph")
- US.add_rule({"highway": "unclassified"}, "55 mph")
- US.add_rule({"highway": "residential"}, "25 mph")
- US.add_rule({"highway": "service"}, "25 mph")
- US.add_rule({"highway": "motorway_link"}, "55 mph")
- US.add_rule({"highway": "trunk_link"}, "55 mph")
- US.add_rule({"highway": "primary_link"}, "55 mph")
- US.add_rule({"highway": "secondary_link"}, "45 mph")
- US.add_rule({"highway": "tertiary_link"}, "35 mph")
- US.add_rule({"highway": "living_street"}, "15 mph")
-
- """ States """
- new_york = US.add_region("New York") # Fourth step, add a state/region to country
- new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules
- new_york.add_rule({"highway": "secondary"}, "55 mph")
- new_york.add_rule({"highway": "tertiary"}, "55 mph")
- new_york.add_rule({"highway": "residential"}, "30 mph")
- new_york.add_rule({"highway": "primary_link"}, "45 mph")
- new_york.add_rule({"highway": "secondary_link"}, "55 mph")
- new_york.add_rule({"highway": "tertiary_link"}, "55 mph")
- # All if not written by the state, the rules will default to the country rules
-
- #california = US.add_region("California")
- # California uses only the default US rules
-
- michigan = US.add_region("Michigan")
- michigan.add_rule({"highway": "motorway"}, "70 mph")
-
- oregon = US.add_region("Oregon")
- oregon.add_rule({"highway": "motorway"}, "55 mph")
- oregon.add_rule({"highway": "secondary"}, "35 mph")
- oregon.add_rule({"highway": "tertiary"}, "30 mph")
- oregon.add_rule({"highway": "service"}, "15 mph")
- oregon.add_rule({"highway": "secondary_link"}, "35 mph")
- oregon.add_rule({"highway": "tertiary_link"}, "30 mph")
-
- south_dakota = US.add_region("South Dakota")
- south_dakota.add_rule({"highway": "motorway"}, "80 mph")
- south_dakota.add_rule({"highway": "trunk"}, "70 mph")
- south_dakota.add_rule({"highway": "primary"}, "65 mph")
- south_dakota.add_rule({"highway": "trunk_link"}, "70 mph")
- south_dakota.add_rule({"highway": "primary_link"}, "65 mph")
-
- wisconsin = US.add_region("Wisconsin")
- wisconsin.add_rule({"highway": "trunk"}, "65 mph")
- wisconsin.add_rule({"highway": "tertiary"}, "45 mph")
- wisconsin.add_rule({"highway": "unclassified"}, "35 mph")
- wisconsin.add_rule({"highway": "trunk_link"}, "65 mph")
- wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph")
-
- """
- --------------------------------------------------
- AU - Australia
- --------------------------------------------------
- """
- AU = Country("AU")
- countries.append(AU)
-
- """ Default rules """
- AU.add_rule({"highway": "motorway"}, "100")
- AU.add_rule({"highway": "trunk"}, "80")
- AU.add_rule({"highway": "primary"}, "80")
- AU.add_rule({"highway": "secondary"}, "50")
- AU.add_rule({"highway": "tertiary"}, "50")
- AU.add_rule({"highway": "unclassified"}, "80")
- AU.add_rule({"highway": "residential"}, "50")
- AU.add_rule({"highway": "service"}, "40")
- AU.add_rule({"highway": "motorway_link"}, "90")
- AU.add_rule({"highway": "trunk_link"}, "80")
- AU.add_rule({"highway": "primary_link"}, "80")
- AU.add_rule({"highway": "secondary_link"}, "50")
- AU.add_rule({"highway": "tertiary_link"}, "50")
- AU.add_rule({"highway": "living_street"}, "30")
-
- """
- --------------------------------------------------
- CA - Canada
- --------------------------------------------------
- """
- CA = Country("CA")
- countries.append(CA)
-
- """ Default rules """
- CA.add_rule({"highway": "motorway"}, "100")
- CA.add_rule({"highway": "trunk"}, "80")
- CA.add_rule({"highway": "primary"}, "80")
- CA.add_rule({"highway": "secondary"}, "50")
- CA.add_rule({"highway": "tertiary"}, "50")
- CA.add_rule({"highway": "unclassified"}, "80")
- CA.add_rule({"highway": "residential"}, "40")
- CA.add_rule({"highway": "service"}, "40")
- CA.add_rule({"highway": "motorway_link"}, "90")
- CA.add_rule({"highway": "trunk_link"}, "80")
- CA.add_rule({"highway": "primary_link"}, "80")
- CA.add_rule({"highway": "secondary_link"}, "50")
- CA.add_rule({"highway": "tertiary_link"}, "50")
- CA.add_rule({"highway": "living_street"}, "20")
-
-
- """
- --------------------------------------------------
- DE - Germany
- --------------------------------------------------
- """
- DE = Country("DE")
- countries.append(DE)
-
- """ Default rules """
- DE.add_rule({"highway": "motorway"}, "none")
- DE.add_rule({"highway": "living_street"}, "10")
- DE.add_rule({"highway": "residential"}, "30")
- DE.add_rule({"zone:traffic": "DE:rural"}, "100")
- DE.add_rule({"zone:traffic": "DE:urban"}, "50")
- DE.add_rule({"zone:maxspeed": "DE:30"}, "30")
- DE.add_rule({"zone:maxspeed": "DE:urban"}, "50")
- DE.add_rule({"zone:maxspeed": "DE:rural"}, "100")
- DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none")
- DE.add_rule({"bicycle_road": "yes"}, "30")
-
-
- """
- --------------------------------------------------
- EE - Estonia
- --------------------------------------------------
- """
- EE = Country("EE")
- countries.append(EE)
-
- """ Default rules """
- EE.add_rule({"highway": "motorway"}, "90")
- EE.add_rule({"highway": "trunk"}, "90")
- EE.add_rule({"highway": "primary"}, "90")
- EE.add_rule({"highway": "secondary"}, "50")
- EE.add_rule({"highway": "tertiary"}, "50")
- EE.add_rule({"highway": "unclassified"}, "90")
- EE.add_rule({"highway": "residential"}, "40")
- EE.add_rule({"highway": "service"}, "40")
- EE.add_rule({"highway": "motorway_link"}, "90")
- EE.add_rule({"highway": "trunk_link"}, "70")
- EE.add_rule({"highway": "primary_link"}, "70")
- EE.add_rule({"highway": "secondary_link"}, "50")
- EE.add_rule({"highway": "tertiary_link"}, "50")
- EE.add_rule({"highway": "living_street"}, "20")
-
-
- """ --- DO NOT MODIFY CODE BELOW THIS LINE --- """
- """ --- ADD YOUR COUNTRY OR STATE ABOVE --- """
-
- # Final step
- write_json(countries, filename)
-
-def write_json(countries, filename = DEFAULT_OUTPUT_FILENAME):
- out_dict = {}
- for country in countries:
- out_dict.update(country.jsonify())
- json_string = json.dumps(out_dict, indent=2)
- with open(filename, "wb") as f:
- f.write(json_string)
-
-
-class Region(object):
- ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road", "zone:maxspeed"]
- ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"]
- def __init__(self, name):
- self.name = name
- self.rules = []
-
- def add_rule(self, tag_conditions, speed):
- new_rule = {}
- if not isinstance(tag_conditions, dict):
- raise TypeError("Rule tag conditions must be dictionary")
- if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions):
- raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS
- if 'highway' in tag_conditions:
- if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES:
- raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"]))
- new_rule['tags'] = tag_conditions
- try:
- new_rule['speed'] = str(speed)
- except ValueError:
- raise ValueError("Rule speed must be string")
- self.rules.append(new_rule)
-
- def jsonify(self):
- ret_dict = {}
- ret_dict[self.name] = self.rules
- return ret_dict
-
-class Country(Region):
- ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZM","ZW"]
- def __init__(self, ISO_3166_alpha_2):
- Region.__init__(self, ISO_3166_alpha_2)
- if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES:
- raise ValueError("Not valid IOS 3166 country code")
- self.regions = {}
-
- def add_region(self, name):
- self.regions[name] = Region(name)
- return self.regions[name]
-
- def jsonify(self):
- ret_dict = {}
- ret_dict[self.name] = {}
- for r_name, region in self.regions.items():
- ret_dict[self.name].update(region.jsonify())
- ret_dict[self.name]['Default'] = self.rules
- return ret_dict
-
-
-if __name__ == '__main__':
- main()
diff --git a/selfdrive/mapd/lib/NodesData.py b/selfdrive/mapd/lib/NodesData.py
deleted file mode 100644
index 2f30607df..000000000
--- a/selfdrive/mapd/lib/NodesData.py
+++ /dev/null
@@ -1,393 +0,0 @@
-import numpy as np
-from enum import Enum
-from selfdrive.mapd.lib.geo import DIRECTION, R, vectors
-
-# from scipy.interpolate import splev, splprep
-from opspline import splev, splprep # pylint: disable=E0401
-
-
-_TURN_CURVATURE_THRESHOLD = 0.002 # 1/mts. A curvature over this value will generate a speed limit section.
-_MAX_LAT_ACC = 2.3 # Maximum lateral acceleration in turns.
-_SPLINE_EVAL_STEP = 5 # mts for spline evaluation for curvature calculation
-_MIN_SPEED_SECTION_LENGTH = 100. # mts. Sections below this value will not be split in smaller sections.
-_MAX_CURV_DEVIATION_FOR_SPLIT = 2. # Split a speed section if the max curvature deviates from mean by this factor.
-_MAX_CURV_SPLIT_ARC_ANGLE = 90. # degrees. Arc section to split into new speed section around max curvature.
-_MIN_NODE_DISTANCE = 50. # mts. Minimum distance between nodes for spline evaluation. Data is enhanced if not met.
-_ADDED_NODES_DIST = 15. # mts. Distance between added nodes when data is enhanced for spline evaluation.
-_DIVERTION_SEARCH_RANGE = [-200., 50.] # mt. Range of distance to current location for divertion search.
-
-
-def nodes_raw_data_array_for_wr(wr, drop_last=False):
- """Provides an array of raw node data (id, lat, lon, speed_limit) for all nodes in way relation
- """
- sl = wr.speed_limit
- data = np.array([(n.id, n.lat, n.lon, sl) for n in wr.way.nodes], dtype=float)
-
- # reverse the order if way direction is backwards
- if wr.direction == DIRECTION.BACKWARD:
- data = np.flip(data, axis=0)
-
- # drop last if requested
- return data[:-1] if drop_last else data
-
-
-def node_calculations(points):
- """Provides node calculations based on an array of (lat, lon) points in radians.
- points is a (N x 1) array where N >= 3
- """
- if len(points) < 3:
- raise(IndexError)
-
- # Get the vector representation of node points in cartesian plane.
- # (N-1, 2) array. Not including (0., 0.)
- v = vectors(points) * R
-
- # Calculate the vector magnitudes (or distance)
- # (N-1, 1) array. No distance for v[-1]
- d = np.linalg.norm(v, axis=1)
-
- # Calculate the bearing (from true north clockwise) for every node.
- # (N-1, 1) array. No bearing for v[-1]
- b = np.arctan2(v[:, 0], v[:, 1])
-
- # Add origin to vector space. (i.e first node in list)
- v = np.concatenate(([[0., 0.]], v))
-
- # Provide distance to previous node and distance to next node
- dp = np.concatenate(([0.], d))
- dn = np.concatenate((d, [0.]))
-
- # Provide cumulative distance on route
- dr = np.cumsum(dp, axis=0)
-
- # Bearing of last node should keep bearing from previous.
- b = np.concatenate((b, [b[-1]]))
-
- return v, dp, dn, dr, b
-
-
-def spline_curvature_calculations(vect, dist_prev):
- """Provides an array of curvatures and its distances by applying a spline interpolation
- to the path described by the nodes data.
- """
- # We need to artificially enhance the data before applying spline interpolation to avoid getting
- # inexistent curvature values close to irregularities on the road when the resolution of nodes data
- # approaching the irregularity is low.
-
- # - Find indexes where dist_prev is greater than threshold
- too_far_idxs = np.nonzero(dist_prev >= _MIN_NODE_DISTANCE)[0]
-
- # - Traversing in reverse order, enhance data by adding points at the found indexes.
- for idx in too_far_idxs[::-1]:
- dp = dist_prev[idx] # distance of vector that needs to be replaced by higher resolution vectors.
- n = int(np.ceil(dp / _ADDED_NODES_DIST)) # number of vectors that need to be added.
- new_v = vect[idx, :] / n # new relative vector to insert.
- vect = np.delete(vect, idx, axis=0) # remove the relative vector to be replaced by the insertion of new vectors.
- vect = np.insert(vect, [idx] * n, [new_v] * n, axis=0) # insert n new relative vectors
-
- # Data is now enhanced, we can proceed with curvature evaluation.
- # - Create cumulative arrays for distance traveled and vector (x, y)
- ds = np.cumsum(dist_prev, axis=0)
- vs = np.cumsum(vect, axis=0)
-
- # - spline interpolation
- tck, u = splprep([vs[:, 0], vs[:, 1]]) # pylint: disable=unbalanced-tuple-unpacking
-
- # - evaluate every _SPLINE_EVAL_STEP mts.
- n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u))
- unew = np.arange(0, n + 1) / n
-
- # - get derivatives
- d1 = splev(unew, tck, der=1)
- d2 = splev(unew, tck, der=2)
-
- # - calculate curvatures
- num = d1[0] * d2[1] - d1[1] * d2[0]
- den = (d1[0]**2 + d1[1]**2)**(1.5)
- curv = num / den
- curv_ds = unew * ds[-1]
-
- return curv, curv_ds
-
-
-def speed_section(curv_sec):
- """Map curvature section data into turn speed sections data.
- Returns: [section start distance, section end distance, speed limit based on max curvature, sing of curvature]
- """
- max_curv_idx = np.argmax(curv_sec[:, 0])
- start = np.amin(curv_sec[:, 2])
- end = np.amax(curv_sec[:, 2])
-
- return np.array([start, end, np.sqrt(_MAX_LAT_ACC / curv_sec[max_curv_idx, 0]), curv_sec[max_curv_idx, 1]])
-
-
-def split_speed_section_by_sign(curv_sec):
- """Will split the given curvature section in subsections if there is a change of sign on the curvature value
- in the section.
- """
- # Find the indexes where the curvatures change signs (if any).
- c_idx = np.nonzero(np.diff(curv_sec[:, 1]))[0] + 1
-
- # Split section base on change of sign.
- return np.split(curv_sec, c_idx)
-
-
-def split_speed_section_by_curv_degree(curv_sec):
- """Will split the given curvature section in subsections as to isolate peaks of turn with substantially
- higher curvature values. This will aid on preventing having very long turn sections with low speed limit
- that is only really necessary for a small region of the section.
- """
- # Only consider spliting a section if long enough.
- length = curv_sec[-1, 2] - curv_sec[0, 2]
- if length <= _MIN_SPEED_SECTION_LENGTH:
- return [curv_sec]
-
- # Only split if max curvature deviates substantially from mean curvature.
- max_curv_idx = np.argmax(curv_sec[:, 0])
- max_curv = curv_sec[max_curv_idx, 0]
- mean_curv = np.mean(curv_sec[:, 0])
- if max_curv / mean_curv <= _MAX_CURV_DEVIATION_FOR_SPLIT:
- return [curv_sec]
-
- # Calcualate where to split as to isolate a curve section around the max curvature peak.
- arc_side = (np.radians(_MAX_CURV_SPLIT_ARC_ANGLE) / max_curv) / 2.
- arc_side_idx_lenght = int(np.ceil(arc_side / _SPLINE_EVAL_STEP))
- split_idxs = [max_curv_idx - arc_side_idx_lenght, max_curv_idx + arc_side_idx_lenght]
- split_idxs = list(filter(lambda idx: idx > 0 and idx < len(curv_sec) - 1, split_idxs))
-
- # If the arc section to split extendes outside the section, then no need to split.
- if len(split_idxs) == 0:
- return [curv_sec]
-
- # Create the splits and split the resulting sections recursevly.
- splits = [split_speed_section_by_curv_degree(cs) for cs in np.split(curv_sec, split_idxs)]
-
- # Flatten the results and return the new list of curvature sections.
- curv_secs = [cs for split in splits for cs in split]
- return curv_secs
-
-
-def speed_limits_for_curvatures_data(curv, dist):
- """Provides the calculations for the speed limits from the curvatures array and distances,
- by providing distances to curvature sections and correspoinding speed limit values as well as
- curvature direction/sign.
- """
- # Prepare a data array for processing with absolute curvature values, curvature sign and distances.
- curv_abs = np.abs(curv)
- data = np.column_stack((curv_abs, np.sign(curv), dist))
-
- # Find where curvatures overshoot turn curvature threshold and define as section
- is_section = curv_abs >= _TURN_CURVATURE_THRESHOLD
-
- # Find the indexes where the sections start and end. i.e. change indexes.
- c_idx = np.nonzero(np.diff(is_section))[0] + 1
-
- # Create independent arrays for each split section base on change indexes.
- splits = np.array(np.split(data, c_idx), dtype=object)
-
- # Filter the splits to keep only the curvature section arrays by getting the odd or even split arrays depending
- # on whether the first split is a curvature split or not.
- curv_sec_idxs = np.arange(0 if is_section[0] else 1, len(splits), 2, dtype=int)
- curv_secs = splits[curv_sec_idxs]
-
- # Further split the curv sections by sign change
- sub_secs = [split_speed_section_by_sign(cs) for cs in curv_secs]
- curv_secs = [cs for sub_sec in sub_secs for cs in sub_sec]
-
- # Further split the curv sections by degree of curvature
- sub_secs = [split_speed_section_by_curv_degree(cs) for cs in curv_secs]
- curv_secs = [cs for sub_sec in sub_secs for cs in sub_sec]
-
- # Return an array where each row represents a turn speed limit section.
- # [start, end, speed_limit, curvature_sign]
- return np.array([speed_section(cs) for cs in curv_secs])
-
-def is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids):
- """
- Evaluates if the way relation `wr` is a valid divertion from node with id `node_id`.
- A valid divertion is a way relation with an edge node with the given `node_id` that is not already included
- in the list of way relations in the route (`wr_ids`) and that can be travaled in the direction as if starting
- from node with id `node_id`
- """
- if wr.id in wr_ids:
- return False
- wr.update_direction_from_starting_node(node_id)
- return not wr.is_prohibited
-
-
-class SpeedLimitSection():
- """And object representing a speed limited road section ahead.
- provides the start and end distance and the speed limit value
- """
- def __init__(self, start, end, value):
- self.start = start
- self.end = end
- self.value = value
-
- def __repr__(self):
- return f'from: {self.start}, to: {self.end}, limit: {self.value}'
-
-
-class TurnSpeedLimitSection(SpeedLimitSection):
- def __init__(self, start, end, value, sign):
- super().__init__(start, end, value)
- self.curv_sign = sign
-
- def __repr__(self):
- return f'{super().__repr__()}, sign: {self.curv_sign}'
-
-
-class NodeDataIdx(Enum):
- """Column index for data elements on NodesData underlying data store.
- """
- node_id = 0
- lat = 1
- lon = 2
- speed_limit = 3
- x = 4 # x value of cartesian vector representing the section between last node and this node.
- y = 5 # y value of cartesian vector representing the section between last node and this node.
- dist_prev = 6 # distance to previous node.
- dist_next = 7 # distance to next node
- dist_route = 8 # cumulative distance on route
- bearing = 9 # bearing of the vector departing from this node.
-
-
-class NodesData:
- """Container for the list of node data from a ordered list of way relations to be used in a Route
- """
- def __init__(self, way_relations, wr_index):
- self._nodes_data = np.array([])
- self._divertions = [[]]
- self._curvature_speed_sections_data = np.array([])
-
- way_count = len(way_relations)
- if way_count == 0:
- return
-
- # We want all the nodes from the last way section
- nodes_data = nodes_raw_data_array_for_wr(way_relations[-1])
-
- # For the ways before the last in the route we want all the nodes but the last, as that one is the first on
- # the next section. Collect them, append last way node data and concatenate the numpy arrays.
- if way_count > 1:
- wrs_data = tuple([nodes_raw_data_array_for_wr(wr, drop_last=True) for wr in way_relations[:-1]])
- wrs_data += (nodes_data,)
- nodes_data = np.concatenate(wrs_data)
-
- # Get a subarray with lat, lon to compute the remaining node values.
- lat_lon_array = nodes_data[:, [1, 2]]
- points = np.radians(lat_lon_array)
- # Ensure we have more than 3 points, if not calculations are not possible.
- if len(points) <= 3:
- return
- vect, dist_prev, dist_next, dist_route, bearing = node_calculations(points)
-
- # append calculations to nodes_data
- # nodes_data structure: [id, lat, lon, speed_limit, x, y, dist_prev, dist_next, dist_route, bearing]
- self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, dist_route, bearing))
-
- # Build route divertion options data from the wr_index.
- wr_ids = [wr.id for wr in way_relations]
- self._divertions = [[wr for wr in wr_index.way_relations_with_edge_node_id(node_id)
- if is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids)]
- for node_id in nodes_data[:, 0]]
-
- # Store calculcations for curvature sections speed limits. We need more than 3 points to be able to process.
- # _curvature_speed_sections_data structure: [dist_start, dist_stop, speed_limits, curv_sign]
- if len(vect) > 3:
- curv, curv_ds = spline_curvature_calculations(vect, dist_prev)
- self._curvature_speed_sections_data = speed_limits_for_curvatures_data(curv, curv_ds)
-
- @property
- def count(self):
- return len(self._nodes_data)
-
- def get(self, node_data_idx):
- """Returns the array containing all the elements of a specific NodeDataIdx type.
- """
- if len(self._nodes_data) == 0 or node_data_idx.value >= self._nodes_data.shape[1]:
- return np.array([])
-
- return self._nodes_data[:, node_data_idx.value]
-
- def speed_limits_ahead(self, ahead_idx, distance_to_node_ahead):
- """Returns and array of SpeedLimitSection objects for the actual route ahead of current location
- """
- if len(self._nodes_data) == 0 or ahead_idx is None:
- return []
-
- # Find the cumulative distances where speed limit changes. Build Speed limit sections for those.
- dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:]))
- dist = np.cumsum(dist, axis=0)
- sl = self.get(NodeDataIdx.speed_limit)[ahead_idx - 1:]
- sl_next = np.concatenate((sl[1:], [0.]))
-
- # Create a boolean mask where speed limit changes and filter values
- sl_change = sl != sl_next
- distances = dist[sl_change]
- speed_limits = sl[sl_change]
-
- # Create speed limits sections combining all continious nodes that have same speed limit value.
- start = 0.
- limits_ahead = []
- for idx, end in enumerate(distances):
- limits_ahead.append(SpeedLimitSection(start, end, speed_limits[idx]))
- start = end
-
- return limits_ahead
-
- def distance_to_end(self, ahead_idx, distance_to_node_ahead):
- if len(self._nodes_data) == 0 or ahead_idx is None:
- return None
-
- return np.sum(np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])))
-
- def curvatures_speed_limit_sections_ahead(self, ahead_idx, distance_to_node_ahead):
- """Returns and array of TurnSpeedLimitSection objects for the actual route ahead of current location for
- speed limit sections due to curvatures in the road.
- """
- if len(self._curvature_speed_sections_data) == 0 or ahead_idx is None:
- return []
-
- # Find the current distance traveled so far on the route.
- dist_curr = self.get(NodeDataIdx.dist_route)[ahead_idx] - distance_to_node_ahead
-
- # Filter the sections to get only those where the stop distance is ahead of current.
- sec_filter = self._curvature_speed_sections_data[:, 1] > dist_curr
- data = self._curvature_speed_sections_data[sec_filter]
-
- # Offset distances to current distance.
- data[:, [0, 1]] -= dist_curr
-
- # Create speed limits sections
- limits_ahead = [TurnSpeedLimitSection(max(0., d[0]), d[1], d[2], d[3]) for d in data]
-
- return limits_ahead
-
- def possible_divertions(self, ahead_idx, distance_to_node_ahead):
- """ Returns and array with the way relations the route could possible divert to by finding
- the alternative way divertions on the nodes in the vicinity of the current location.
- """
- if len(self._nodes_data) == 0 or ahead_idx is None:
- return []
-
- dist_route = self.get(NodeDataIdx.dist_route)
- rel_dist = dist_route - dist_route[ahead_idx] + distance_to_node_ahead
- valid_idxs = np.nonzero(np.logical_and(rel_dist >= _DIVERTION_SEARCH_RANGE[0],
- rel_dist <= _DIVERTION_SEARCH_RANGE[1]))[0]
- valid_divertions = [self._divertions[i] for i in valid_idxs]
-
- return [wr for wrs in valid_divertions for wr in wrs] # flatten.
-
- def distance_to_node(self, node_id, ahead_idx, distance_to_node_ahead):
- """
- Provides the distance to a specific node in the route identified by `node_id` in reference to the node ahead
- (`ahead_idx`) and the distance from current location to the node ahead (`distance_to_node_ahead`).
- """
- node_ids = self.get(NodeDataIdx.node_id)
- node_idxs = np.nonzero(node_ids == node_id)[0]
- if len(self._nodes_data) == 0 or ahead_idx is None or len(node_idxs) == 0:
- return None
-
- return self.get(NodeDataIdx.dist_route)[node_idxs[0]] - self.get(NodeDataIdx.dist_route)[ahead_idx] + \
- distance_to_node_ahead
diff --git a/selfdrive/mapd/lib/Route.py b/selfdrive/mapd/lib/Route.py
deleted file mode 100644
index 7aa502bb2..000000000
--- a/selfdrive/mapd/lib/Route.py
+++ /dev/null
@@ -1,340 +0,0 @@
-from selfdrive.mapd.lib.NodesData import NodesData, NodeDataIdx
-from selfdrive.mapd.config import QUERY_RADIUS
-from selfdrive.mapd.lib.geo import ref_vectors, R, distance_to_points
-from itertools import compress
-import numpy as np
-
-
-_ACCEPTABLE_BEARING_DELTA_COSINE = -0.7 # Continuation paths with a bearing of 180 +/- 45 degrees.
-_MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE = -0.3420 # bearing delta at route edge must be 180 +/- 70 degrees.
-_MAP_DATA_EDGE_DISTANCE = 50 # mts. Consider edge of map data from this distance to edge of query radius.
-
-
-class Route():
- """A set of consecutive way relations forming a default driving route.
- """
- def __init__(self, current, wr_index, way_collection_id, query_center):
- """Create a Route object from a given `wr_index` (Way relation index)
-
- Args:
- current (WayRelation): The Way Relation that is currently located. It must be active.
- wr_index (WayRelationIndex): The indexes of WayRelations by node id.
- way_collection_id (UUID): The id of the Way Collection that created this Route.
- query_center (Numpy Array): lat, lon] numpy array in radians indicating the center of the data query.
- """
- self.way_collection_id = way_collection_id
- self._ordered_way_relations = []
- self._nodes_data = None
- self._reset()
-
- # An active current way is needed to be able to build a route
- if not current.active:
- return
-
- # Build the route by finding iteratavely the best matching ways continuing after the end of the
- # current (last_wr) way. Use the index to find the continuation posibilities on each iteration.
- last_wr = current
- ordered_way_ids = []
- split_wrs = []
- while True:
- # - Append current element to the route list of ordered way relations.
- self._ordered_way_relations.append(last_wr)
- ordered_way_ids.append(last_wr.id)
-
- # - Get the id of the node at the end of the way and then fetch the way relations that share the end node id.
- last_node_id = last_wr.last_node.id
- way_relations = wr_index.way_relations_with_edge_node_id(last_node_id)
-
- # - Add split way relations when necessary and remove parent way relations.
- split_wrs_to_add = [wr for wr in split_wrs if last_node_id in wr.edge_nodes_ids]
- way_relations.extend(split_wrs_to_add)
- parent_ids = [wr.parent_wr_id for wr in split_wrs_to_add]
- way_relations = [wr for wr in way_relations if wr.id not in parent_ids]
-
- # - If no more way_relations than last_wr, we have to check if we join another wr on an internal node, and
- # if we do, we replace such way relation with the split of it and continue.
- if len(way_relations) == 1:
- way_relations = wr_index.way_relations_with_node_id(last_node_id)
- # If no more way_relations than last_wr or its parent, we got to the end.
- if len(way_relations) == 1:
- break
-
- # If last_wr is a split, replace its parent with last_wr
- way_relations = [last_wr if wr is last_wr.parent else wr for wr in way_relations]
-
- # If we join a wr on an internal node, then we artificially split the wr in two and pass both wrs as
- # candidates to the wr selection code below.
- wr_to_split = [wr for wr in way_relations if wr is not last_wr][0]
- next_split_way_id = -len(split_wrs) - 1 # Keep split wrs ids unique on Route
- new_wrs = wr_to_split.split(last_node_id, [next_split_way_id, next_split_way_id - 1])
- # If it could not be splited, we are done.
- if len(new_wrs) != 2:
- break
-
- # Replace the original way relation for the splitted version on way_relations and track splited wrs.
- split_wrs.extend(new_wrs)
- way_relations.remove(wr_to_split)
- way_relations.extend(new_wrs)
-
- # - Get the coordinates for the edge node and build the array of coordinates for the nodes before the edge node
- # on each of the common way relations, then get the vectors in cartesian plane for the end sections of each way.
- ref_point = last_wr.last_node_coordinates
- points = np.array([wr.node_before_edge_coordinates(last_node_id) for wr in way_relations])
- v = ref_vectors(ref_point, points) * R
-
- # - Calculate the bearing (from true north clockwise) for every end section of each way.
- b = np.arctan2(v[:, 0], v[:, 1])
-
- # - Find index of las_wr section and calculate deltas of bearings to the other sections.
- last_wr_idx = way_relations.index(last_wr)
- b_ref = b[last_wr_idx]
- delta = b - b_ref
-
- # - Update the direction of the possible route continuation ways as starting from last_node_id.
- # Make sure to exclude any ways already included in the ordered list as to not modify direction when there
- # are looping roads (like roundabouts). A way will never be included twice in a route anyway.
- for wr in way_relations:
- if wr.id not in ordered_way_ids:
- wr.update_direction_from_starting_node(last_node_id)
-
- # - Filter the possible route continuation way relations:
- # - exclude any way already added to the ordered list.
- # - exclude all way relations that are prohibited due to traffic direction.
- mask = [wr.id not in ordered_way_ids and not wr.is_prohibited for wr in way_relations]
- way_relations = list(compress(way_relations, mask))
- delta = delta[mask]
-
- # if no options left, we got to the end.
- if len(way_relations) == 0:
- break
-
- # - The cosine of the bearing delta will aid us in choosing the way that continues. The cosine is
- # minimum (-1) for a perfect straight continuation as delta would be pi or -pi.
- cos_delta = np.cos(delta)
-
- def pick_best_idx(cos_delta):
- """Selects the best index on `cos_delta` array for a way that continues the route.
- In principle we want to choose the way that continues as straight as possible.
- Bue we need to make sure that if there are 2 or more ways continuing relatively straight, then we
- need to disambiguate, either by matching the `ref` or `name` value of the continuing way with the
- last way selected.
- This can prevent cases where the chosen route could be for instance an exit ramp of a way due to the fact
- that the ramp has a better match on bearing to previous way. We choose to stay on the road with the same `ref`
- or `name` value if available.
- If there is no ambiguity or there are no `name` or `ref` values to disambiguate, then we pick the one with
- the straightest following direction.
- """
- # Find the indexes of the cosine of the deltas that are considered straight enough to continue.
- idxs = np.nonzero(cos_delta < _ACCEPTABLE_BEARING_DELTA_COSINE)[0]
-
- # If no amiguity or no way to break it, just return the straightest line.
- if len(idxs) <= 1 or (last_wr.ref is None and last_wr.name is None):
- # The section with the best continuation is the one with a bearing delta closest to pi. This is equivalent
- # to taking the one with the smallest cosine of the bearing delta, as cosine is minimum (-1) on both pi
- # and -pi.
- return np.argmin(cos_delta)
-
- wrs = [way_relations[idx] for idx in idxs]
-
- # If we find a continuation way with the same reference we just choose it.
- refs = list(map(lambda wr: wr.ref, wrs))
- if last_wr.ref is not None:
- idx = next((idx for idx, ref in enumerate(refs) if ref == last_wr.ref), None)
- if idx is not None:
- return idxs[idx]
-
- # If we find a continuation way with the same name we just choose it.
- names = list(map(lambda wr: wr.name, wrs))
- if last_wr.name is not None:
- idx = next((idx for idx, name in enumerate(names) if name == last_wr.name), None)
- if idx is not None:
- return idxs[idx]
-
- # We did not manage to deambiguate, choose straightest path.
- return np.argmin(cos_delta)
-
- # Get the index of the continuation way.
- best_idx = pick_best_idx(cos_delta)
-
- # - Make sure to not select as route continuation a way that turns too much if we are close to the border of
- # map data queried. This is to avoid building a route that takes a sharp turn just because we do not have the
- # data for the way that actually continues straight.
- if cos_delta[best_idx] > _MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE:
- dist_to_center = distance_to_points(query_center, np.array([ref_point]))[0]
- if dist_to_center > QUERY_RADIUS - _MAP_DATA_EDGE_DISTANCE:
- break
-
- # - Select next way.
- last_wr = way_relations[best_idx]
-
- # Build the node data from the ordered list of way relations
- self._nodes_data = NodesData(self._ordered_way_relations, wr_index)
-
- # Locate where we are in the route node list.
- self._locate()
-
- def __repr__(self):
- count = self._nodes_data.count if self._nodes_data is not None else None
- return f'Route: {self.way_collection_id}, idx ahead: {self._ahead_idx} of {count}'
-
- def _reset(self):
- self._limits_ahead = None
- self._cuvature_limits_ahead = None
- self._curvatures_ahead = None
- self._ahead_idx = None
- self._distance_to_node_ahead = None
-
- @property
- def located(self):
- return self._ahead_idx is not None
-
- def _locate(self):
- """Will resolve the index in the nodes_data list for the node ahead of the current location.
- It updates as well the distance from the current location to the node ahead.
- """
- current = self.current_wr
- if current is None:
- return
-
- node_ahead_id = current.node_ahead.id
- self._distance_to_node_ahead = current.distance_to_node_ahead
- start_idx = self._ahead_idx if self._ahead_idx is not None else 1
- self._ahead_idx = None
-
- ids = self._nodes_data.get(NodeDataIdx.node_id)
- for idx in range(start_idx, len(ids)):
- if ids[idx] == node_ahead_id:
- self._ahead_idx = idx
- break
-
- @property
- def current_wr(self):
- return self._ordered_way_relations[0] if len(self._ordered_way_relations) else None
-
- def update(self, location_rad, bearing_rad, location_stdev):
- """Will update the route structure based on the given `location_rad` and `bearing_rad` assuming progress on the
- route on the original direction. If direction has changed or active point on the route can not be found, the route
- will become invalid.
- """
- if len(self._ordered_way_relations) == 0 or location_rad is None or bearing_rad is None:
- return
-
- # Skip if no update on location or bearing.
- if np.array_equal(self.current_wr.location_rad, location_rad) and self.current_wr.bearing_rad == bearing_rad:
- return
-
- # Transverse the way relations on the actual order until we find an active one. From there, rebuild the route
- # with the way relations remaining ahead.
- for idx, wr in enumerate(self._ordered_way_relations):
- active_direction = wr.direction
- wr.update(location_rad, bearing_rad, location_stdev)
-
- if not wr.active:
- continue
-
- if wr.direction != active_direction:
- # Driving direction on the route has changed. stop.
- break
-
- # We have now the current wr. Repopulate from here till the end and locate
- self._ordered_way_relations = self._ordered_way_relations[idx:]
- self._reset()
- self._locate()
-
- # If the active way is diverting, check whether there are posibilities to divert from the route in the
- # vecinity of the current location. If there are possibilities, then stop here to loose the route as we are
- # most likely driving away. If there are no possibilites, then stick to the route as the diversion is probably
- # just a matter of GPS accuracy. (It can happen after driving under a bridge)
- if wr.diverting and len(self._nodes_data.possible_divertions(self._ahead_idx, self._distance_to_node_ahead)) > 0:
- break
-
- # The current location in route is valid, return.
- return
-
- # if we got here, there is no new active way relation or driving direction has changed. Reset.
- self._reset()
-
- @property
- def speed_limits_ahead(self):
- """Returns and array of SpeedLimitSection objects for the actual route ahead of current location
- """
- if self._limits_ahead is not None:
- return self._limits_ahead
-
- if self._nodes_data is None or self._ahead_idx is None:
- return []
-
- self._limits_ahead = self._nodes_data.speed_limits_ahead(self._ahead_idx, self._distance_to_node_ahead)
- return self._limits_ahead
-
- @property
- def curvature_speed_limits_ahead(self):
- """Returns and array of TurnSpeedLimitSection objects for the actual route ahead of current location due
- to curvatures
- """
- if self._cuvature_limits_ahead is not None:
- return self._cuvature_limits_ahead
-
- if self._nodes_data is None or self._ahead_idx is None:
- return []
-
- self._cuvature_limits_ahead = self._nodes_data. \
- curvatures_speed_limit_sections_ahead(self._ahead_idx, self._distance_to_node_ahead)
-
- return self._cuvature_limits_ahead
-
- @property
- def current_speed_limit(self):
- if not self.located:
- return None
-
- limits_ahead = self.speed_limits_ahead
- if len(limits_ahead) == 0 or limits_ahead[0].start != 0:
- return None
-
- return limits_ahead[0].value
-
- @property
- def current_curvature_speed_limit_section(self):
- if not self.located:
- return None
-
- limits_ahead = self.curvature_speed_limits_ahead
- if len(limits_ahead) == 0 or limits_ahead[0].start != 0:
- return None
-
- return limits_ahead[0]
-
- @property
- def next_speed_limit_section(self):
- if not self.located:
- return None
-
- limits_ahead = self.speed_limits_ahead
- if len(limits_ahead) == 0:
- return None
-
- # Find the first section that does not start in 0. i.e. the next section
- for section in limits_ahead:
- if section.start > 0:
- return section
-
- return None
-
- def next_curvature_speed_limit_sections(self, horizon_mts):
- if not self.located:
- return []
-
- # Provide the curvature speed sections that start ahead (> 0) and up to horizon
- return list(filter(lambda la: la.start > 0 and la.start <= horizon_mts, self.curvature_speed_limits_ahead))
-
- @property
- def distance_to_end(self):
- if not self.located:
- return None
-
- return self._nodes_data.distance_to_end(self._ahead_idx, self._distance_to_node_ahead)
-
- @property
- def current_road_name(self):
- return self.current_wr.road_name if self.located else None
diff --git a/selfdrive/mapd/lib/WayCollection.py b/selfdrive/mapd/lib/WayCollection.py
deleted file mode 100644
index 56ad0b60d..000000000
--- a/selfdrive/mapd/lib/WayCollection.py
+++ /dev/null
@@ -1,85 +0,0 @@
-from selfdrive.mapd.lib.WayRelation import WayRelation
-from selfdrive.mapd.lib.WayRelationIndex import WayRelationIndex
-from selfdrive.mapd.lib.Route import Route
-from selfdrive.mapd.config import LANE_WIDTH
-import uuid
-
-
-_ACCEPTABLE_BEARING_DELTA_IND = 0.7071067811865475 # sin(pi/4) | 45 degrees acceptable bearing delta
-
-
-class WayCollection():
- """A collection of WayRelations to use for maps data analysis.
- """
- def __init__(self, ways, query_center):
- """Creates a WayCollection with a set of OSM way objects.
-
- Args:
- ways (Array): Collection of Way objects fetched from OSM in a radius around `query_center`
- query_center (Numpy Array): [lat, lon] numpy array in radians indicating the center of the data query.
- """
- self.id = uuid.uuid4()
- self.way_relations = [WayRelation(way) for way in ways]
- self.query_center = query_center
-
- self.wr_index = WayRelationIndex(self.way_relations)
-
- def get_route(self, location_rad, bearing_rad, location_stdev):
- """Provides the best route found in the way collection based on current location and bearing.
- """
- if location_rad is None or bearing_rad is None or location_stdev is None:
- return None
-
- # Update all way relations in collection to the provided location and bearing.
- for wr in self.way_relations:
- wr.update(location_rad, bearing_rad, location_stdev)
-
- # Get the way relations where a match was found. i.e. those now marked as active as long as the direction of
- # travel is valid.
- valid_way_relations = [wr for wr in self.way_relations if wr.active and not wr.is_prohibited]
-
- # If no active, then we could not find a current way to build a route.
- if len(valid_way_relations) == 0:
- return None
-
- # If only one valid, then pick it as current.
- if len(valid_way_relations) == 1:
- current = valid_way_relations[0]
-
- # If more than one is valid, filter out any valid way relation where the bearing delta indicator is too high.
- else:
- wr_acceptable_bearing = list(filter(lambda wr: wr.active_bearing_delta <= _ACCEPTABLE_BEARING_DELTA_IND,
- valid_way_relations))
-
- # If delta bearing indicator is too high for all, then use as current the one that has the shorter one.
- if len(wr_acceptable_bearing) == 0:
- valid_way_relations.sort(key=lambda wr: wr.active_bearing_delta)
- current = valid_way_relations[0]
-
- # If only one with acceptable bearing, use it.
- elif len(wr_acceptable_bearing) == 1:
- current = wr_acceptable_bearing[0]
-
- else:
- # If more than one with acceptable bearing, filter the ones with distance to way lower than 2 standard
- # deviation from GPS accuracy (95%) + half the road width estimate.
- wr_accurate_distance = [wr for wr in wr_acceptable_bearing
- if wr.distance_to_way <= 2. * location_stdev + wr.lanes * LANE_WIDTH / 2.]
-
- # If none with accurate distance to way, then select the closest to the way
- if len(wr_accurate_distance) == 0:
- wr_acceptable_bearing.sort(key=lambda wr: wr.distance_to_way)
- current = wr_acceptable_bearing[0]
-
- # If only one with distance under accuracy, select this one.
- elif len(wr_accurate_distance) == 1:
- current = wr_accurate_distance[0]
-
- # If more than one with distance under accuracy. Then select the one with lowest highway rank.
- # i.e. prefere motorways over other roads and so on. This is to prevent selecting a small paralel
- # road to a main road when the accuracy is poor.
- else:
- wr_accurate_distance.sort(key=lambda wr: wr.highway_rank)
- current = wr_accurate_distance[0]
-
- return Route(current, self.wr_index, self.id, self.query_center)
diff --git a/selfdrive/mapd/lib/WayRelation.py b/selfdrive/mapd/lib/WayRelation.py
deleted file mode 100644
index 7c796eb40..000000000
--- a/selfdrive/mapd/lib/WayRelation.py
+++ /dev/null
@@ -1,422 +0,0 @@
-from selfdrive.mapd.lib.geo import DIRECTION, R, vectors, bearing_to_points, distance_to_points
-from selfdrive.mapd.lib.osm import create_way
-from common.conversions import Conversions as CV
-from selfdrive.mapd.config import LANE_WIDTH
-from common.basedir import BASEDIR
-from datetime import datetime as dt
-import numpy as np
-import re
-import json
-
-
-_WAY_BBOX_PADING = 80. / R # 80 mts of pading to bounding box. (expressed in radians)
-
-
-with open(BASEDIR + "/selfdrive/mapd/lib/default_speeds.json", "rb") as f:
- _COUNTRY_LIMITS = json.loads(f.read())
-
-
-_WD = {
- 'Mo': 0,
- 'Tu': 1,
- 'We': 2,
- 'Th': 3,
- 'Fr': 4,
- 'Sa': 5,
- 'Su': 6
-}
-
-_HIGHWAY_RANK = {
- 'motorway': 0,
- 'motorway_link': 1,
- 'trunk': 10,
- 'trunk_link': 11,
- 'primary': 20,
- 'primary_link': 21,
- 'secondary': 30,
- 'secondary_link': 31,
- 'tertiary': 40,
- 'tertiary_link': 41,
- 'unclassified': 50,
- 'residential': 60,
- 'living_street': 61
-}
-
-
-def is_osm_time_condition_active(condition_string):
- """
- Will indicate if a time condition for a restriction as described
- @ https://wiki.openstreetmap.org/wiki/Conditional_restrictions
- is active for the current date and time of day.
- """
- now = dt.now().astimezone()
- today = now.date()
- week_days = []
-
- # Look for days of week matched and validate if today matches criteria.
- dr = re.findall(r'(Mo|Tu|We|Th|Fr|Sa|Su[-,\s]*?)', condition_string)
-
- if len(dr) == 1:
- week_days = [_WD[dr[0]]]
- # If two or more matches condider it a range of days between 1st and 2nd element.
- elif len(dr) > 1:
- week_days = list(range(_WD[dr[0]], _WD[dr[1]] + 1))
-
- # If valid week days list is not empy and today day is not in the list, then the time-date range is not active.
- if len(week_days) > 0 and now.weekday() not in week_days:
- return False
-
- # Look for time ranges on the day. No time range, means all day
- tr = re.findall(r'([0-9]{1,2}:[0-9]{2})\s*?-\s*?([0-9]{1,2}:[0-9]{2})', condition_string)
-
- # if no time range but there were week days set, consider it active during the whole day
- if len(tr) == 0:
- return len(dr) > 0
-
- # Search among time ranges matched, one where now time belongs too. If found range is active.
- for times_tup in tr:
- times = list(map(lambda tt: dt.
- combine(today, dt.strptime(tt, '%H:%M').time().replace(tzinfo=now.tzinfo)), times_tup))
- if now >= times[0] and now <= times[1]:
- return True
-
- return False
-
-
-def speed_limit_value_for_limit_string(limit_string):
- # Look for matches of speed by default in kph, or in mph when explicitly noted.
- v = re.match(r'^\s*([0-9]{1,3})\s*?(mph)?\s*$', limit_string)
- if v is None:
- return None
- conv = CV.MPH_TO_MS if v[2] is not None and v[2] == "mph" else CV.KPH_TO_MS
- return conv * float(v[1])
-
-
-def speed_limit_for_osm_tag_limit_string(limit_string):
- # https://wiki.openstreetmap.org/wiki/Key:maxspeed
- if limit_string is None:
- # When limit is set to 0. is considered not existing.
- return 0.
-
- # Attempt to parse limit as simple numeric value considering units.
- limit = speed_limit_value_for_limit_string(limit_string)
- if limit is not None:
- return limit
-
- # Look for matches of speed with country implicit values.
- v = re.match(r'^\s*([A-Z]{2}):([a-z_]+):?([0-9]{1,3})?(\s+)?(mph)?\s*', limit_string)
- if v is None:
- return 0.
-
- if v[2] == "zone" and v[3] is not None:
- conv = CV.MPH_TO_MS if v[5] is not None and v[5] == "mph" else CV.KPH_TO_MS
- limit = conv * float(v[3])
- elif f'{v[1]}:{v[2]}' in _COUNTRY_LIMITS:
- limit = speed_limit_value_for_limit_string(_COUNTRY_LIMITS[f'{v[1]}:{v[2]}'])
-
- return limit if limit is not None else 0.
-
-
-def conditional_speed_limit_for_osm_tag_limit_string(limit_string):
- if limit_string is None:
- # When limit is set to 0. is considered not existing.
- return 0.
-
- # Look for matches of the ` @ ()` format
- v = re.match(r'^(.*)@\s*\((.*)\).*$', limit_string)
- if v is None:
- return 0. # No valid format match
-
- value = speed_limit_for_osm_tag_limit_string(v[1])
- if value == 0.:
- return 0. # Invalid speed limit value
-
- # Look for date-time conditions separated by semicolon
- v = re.findall(r'(?:;|^)([^;]*)', v[2])
- for datetime_condition in v:
- if is_osm_time_condition_active(datetime_condition):
- return value
-
- # If we get here, no current date-time conditon is active.
- return 0.
-
-
-class WayRelation():
- """A class that represent the relationship of an OSM way and a given `location` and `bearing` of a driving vehicle.
- """
- def __init__(self, way, parent=None):
- self.way = way
- self.parent = parent
- self.parent_wr_id = parent.id if parent is not None else None # For WRs created as splits of other WRs
- self.reset_location_variables()
- self.direction = DIRECTION.NONE
- self._speed_limit = None
- self._one_way = way.tags.get("oneway")
- self.name = way.tags.get('name')
- self.ref = way.tags.get('ref')
- self.highway_type = way.tags.get("highway")
- self.highway_rank = _HIGHWAY_RANK.get(self.highway_type, 1000)
- try:
- self.lanes = int(way.tags.get('lanes'))
- except Exception:
- self.lanes = 2
-
- # Create numpy arrays with nodes data to support calculations.
- self._nodes_np = np.radians(np.array([[nd.lat, nd.lon] for nd in way.nodes], dtype=float))
- self._nodes_ids = np.array([nd.id for nd in way .nodes], dtype=int)
-
- # Get the vectors representation of the segments betwheen consecutive nodes. (N-1, 2)
- v = vectors(self._nodes_np) * R
-
- # Calculate the vector magnitudes (or distance) between nodes. (N-1)
- self._way_distances = np.linalg.norm(v, axis=1)
-
- # Calculate the bearing (from true north clockwise) for every section of the way (vectors between nodes). (N-1)
- self._way_bearings = np.arctan2(v[:, 0], v[:, 1])
-
- # Define bounding box to ease the process of locating a node in a way.
- # [[min_lat, min_lon], [max_lat, max_lon]]
- self.bbox = np.row_stack((np.amin(self._nodes_np, 0) - _WAY_BBOX_PADING,
- np.amax(self._nodes_np, 0) + _WAY_BBOX_PADING))
-
- # Get the edge nodes ids.
- self.edge_nodes_ids = [way.nodes[0].id, way.nodes[-1].id]
-
- def __repr__(self):
- return f'(id: {self.id}, between {self.behind_idx} and {self.ahead_idx}, {self.direction}, active: {self.active})'
-
- def __eq__(self, other):
- if isinstance(other, WayRelation):
- return self.id == other.id
- return False
-
- def reset_location_variables(self):
- self.distance_to_node_ahead = 0.
- self.location_rad = None
- self.bearing_rad = None
- self.active = False
- self.diverting = False
- self.ahead_idx = None
- self.behind_idx = None
- self._active_bearing_delta = None
- self._distance_to_way = None
-
- @property
- def id(self):
- return self.way.id
-
- @property
- def road_name(self):
- if self.name is not None:
- return self.name
- return self.ref
-
- def update(self, location_rad, bearing_rad, location_stdev):
- """Will update and validate the associated way with a given `location_rad` and `bearing_rad`.
- Specifically it will find the nodes behind and ahead of the current location and bearing.
- If no proper fit to the way geometry, the way relation is marked as invalid.
- """
- self.reset_location_variables()
-
- # Ignore if location not in way bounding box
- if not self.is_location_in_bbox(location_rad):
- return
-
- # - Get the distance and bearings from location to all nodes. (N)
- bearings = bearing_to_points(location_rad, self._nodes_np)
- distances = distance_to_points(location_rad, self._nodes_np)
-
- # - Get absolute bearing delta to current driving bearing. (N)
- delta = np.abs(bearing_rad - bearings)
-
- # - Nodes are ahead if the cosine of the delta is positive (N)
- is_ahead = np.cos(delta) >= 0.
-
- # - Possible locations on the way are those where adjacent nodes change from ahead to behind or viceversa.
- possible_idxs = np.nonzero(np.diff(is_ahead))[0]
-
- # - when no possible locations found, then the location is not in this way.
- if len(possible_idxs) == 0:
- return
-
- # - Find then angle formed between the vectors from the current location to consecutive nodes. This is the
- # value of the difference in the bearings of the vectors.
- teta = np.diff(bearings)
-
- # - When two consecutive nodes will be ahead and behind, they will form a triangle with the current location.
- # We find the closest distance to the way by solving the area of the triangle and finding the height (h).
- # We must use the abolute value of the sin of the angle in the formula, which is equivalent to ensure we
- # are considering the smallest of the two angles formed between the two vectors.
- # https://www.mathsisfun.com/algebra/trig-area-triangle-without-right-angle.html
- h = distances[:-1] * distances[1:] * np.abs(np.sin(teta)) / self._way_distances
-
- # - Calculate the delta between driving bearing and way bearings. (N-1)
- bw_delta = self._way_bearings - bearing_rad
-
- # - The absolut value of the sin of `bw_delta` indicates how close the bearings match independent of direction.
- # We will use this value along the distance to the way to aid on way selection. (N-1)
- abs_sin_bw_delta = np.abs(np.sin(bw_delta))
-
- # - Get the delta to way bearing indicators and the distance to the way for the possible locations.
- abs_sin_bw_delta_possible = abs_sin_bw_delta[possible_idxs]
- h_possible = h[possible_idxs]
-
- # - Get the index where the distance to the way is minimum. That is the chosen location.
- min_h_possible_idx = np.argmin(h_possible)
- min_delta_idx = possible_idxs[min_h_possible_idx]
-
- # - If the distance to the way is over 4 standard deviations of the gps accuracy + half the maximum road width
- # estimate, then we are way too far to stick to this way (i.e. we are not on this way anymore)
- half_road_width_estimate = self.lanes * LANE_WIDTH / 2.
- if h_possible[min_h_possible_idx] > 4. * location_stdev + half_road_width_estimate:
- return
-
- # - If the distance to the road is greater than 2 standard deviations of the gps accuracy + half the maximum road
- # width estimate then we are most likely diverting from this route.
- diverting = h_possible[min_h_possible_idx] > 2. * location_stdev + half_road_width_estimate
-
- # Populate location variables with result
- if is_ahead[min_delta_idx]:
- self.direction = DIRECTION.BACKWARD
- self.ahead_idx = min_delta_idx
- self.behind_idx = min_delta_idx + 1
- else:
- self.direction = DIRECTION.FORWARD
- self.ahead_idx = min_delta_idx + 1
- self.behind_idx = min_delta_idx
-
- self._distance_to_way = h[min_delta_idx]
- self._active_bearing_delta = abs_sin_bw_delta_possible[min_h_possible_idx]
- # TODO: The distance to node ahead currently represent the distance from the GPS fix location.
- # It would be perhaps more accurate to use the distance on the projection over the direct line between
- # the two nodes.
- self.distance_to_node_ahead = distances[self.ahead_idx]
- self.active = True
- self.diverting = diverting
- self.location_rad = location_rad
- self.bearing_rad = bearing_rad
- self._speed_limit = None
-
- def update_direction_from_starting_node(self, start_node_id):
- self._speed_limit = None
- if self.edge_nodes_ids[0] == start_node_id:
- self.direction = DIRECTION.FORWARD
- elif self.edge_nodes_ids[-1] == start_node_id:
- self.direction = DIRECTION.BACKWARD
- else:
- self.direction = DIRECTION.NONE
-
- def is_location_in_bbox(self, location_rad):
- """Indicates if a given location is contained in the bounding box surrounding the way.
- self.bbox = [[min_lat, min_lon], [max_lat, max_lon]]
- """
- is_g = np.greater_equal(location_rad, self.bbox[0, :])
- is_l = np.less_equal(location_rad, self.bbox[1, :])
-
- return np.all(np.concatenate((is_g, is_l)))
-
- @property
- def speed_limit(self):
- if self._speed_limit is not None:
- return self._speed_limit
-
- # Get string from corresponding tag, consider conditional limits first.
- limit_string = self.way.tags.get("maxspeed:conditional")
- if limit_string is None:
- if self.direction == DIRECTION.FORWARD:
- limit_string = self.way.tags.get("maxspeed:forward:conditional")
- elif self.direction == DIRECTION.BACKWARD:
- limit_string = self.way.tags.get("maxspeed:backward:conditional")
-
- limit = conditional_speed_limit_for_osm_tag_limit_string(limit_string)
-
- # When no conditional limit set, attempt to get from regular speed limit tags.
- if limit == 0.:
- limit_string = self.way.tags.get("maxspeed")
- if limit_string is None:
- if self.direction == DIRECTION.FORWARD:
- limit_string = self.way.tags.get("maxspeed:forward")
- elif self.direction == DIRECTION.BACKWARD:
- limit_string = self.way.tags.get("maxspeed:backward")
-
- limit = speed_limit_for_osm_tag_limit_string(limit_string)
-
- self._speed_limit = limit
- return self._speed_limit
-
- @property
- def active_bearing_delta(self):
- """Returns the sine of the delta between the current location bearing and the exact
- bearing of the portion of way we are currentluy located at.
- """
- return self._active_bearing_delta
-
- @property
- def is_one_way(self):
- return self._one_way in ['yes'] or self.highway_type in ["motorway"]
-
- @property
- def is_prohibited(self):
- # Direction must be defined to asses this property. Default to `True` if not.
- if self.direction == DIRECTION.NONE:
- return True
- return self.is_one_way and self.direction == DIRECTION.BACKWARD
-
- @property
- def distance_to_way(self):
- """Returns the perpendicular (i.e. minimum) distance between current location and the way
- """
- return self._distance_to_way
-
- @property
- def node_ahead(self):
- return self.way.nodes[self.ahead_idx] if self.ahead_idx is not None else None
-
- @property
- def last_node(self):
- """Returns the last node on the way considering the traveling direction
- """
- if self.direction == DIRECTION.FORWARD:
- return self.way.nodes[-1]
- if self.direction == DIRECTION.BACKWARD:
- return self.way.nodes[0]
- return None
-
- @property
- def last_node_coordinates(self):
- """Returns the coordinates for the last node on the way considering the traveling direction. (in radians)
- """
- if self.direction == DIRECTION.FORWARD:
- return self._nodes_np[-1]
- if self.direction == DIRECTION.BACKWARD:
- return self._nodes_np[0]
- return None
-
- def node_before_edge_coordinates(self, node_id):
- """Returns the coordinates of the node before the edge node identifeid with `node_id`. (in radians)
- """
- if self.edge_nodes_ids[0] == node_id:
- return self._nodes_np[1]
-
- if self.edge_nodes_ids[-1] == node_id:
- return self._nodes_np[-2]
-
- return np.array([0., 0.])
-
- def split(self, node_id, way_ids=None):
- """ Returns and array with the way relations resulting from spliting the current way relation at node_id
- """
- idxs = np.nonzero(self._nodes_ids == node_id)[0]
- if len(idxs) == 0:
- return []
-
- idx = idxs[0]
- if idx == 0 or idx == len(self._nodes_ids) - 1:
- return [self]
-
- if not isinstance(way_ids, list):
- way_ids = [-1, -2] # Default id values.
-
- ways = [create_way(way_ids[0], node_ids=self._nodes_ids[:idx + 1], from_way=self.way),
- create_way(way_ids[1], node_ids=self._nodes_ids[idx:], from_way=self.way)]
- return [WayRelation(way, parent=self) for way in ways]
diff --git a/selfdrive/mapd/lib/WayRelationIndex.py b/selfdrive/mapd/lib/WayRelationIndex.py
deleted file mode 100644
index e941dcd5b..000000000
--- a/selfdrive/mapd/lib/WayRelationIndex.py
+++ /dev/null
@@ -1,34 +0,0 @@
-
-
-class WayRelationIndex():
- """
- A class containing an index of WayRelations by node ids of internal nodes and edge nodes.
- """
- def __init__(self, way_relations):
- self._edge_nodes_index_dict = {}
- self._full_nodes_index_dict = {}
-
- for wr in way_relations:
- self.add(wr)
-
- def add(self, way_relation):
- for node in way_relation.way.nodes:
- node_id = node.id
- self._full_nodes_index_dict[node_id] = self._full_nodes_index_dict.get(node_id, []) + [way_relation]
- if node_id in way_relation.edge_nodes_ids:
- self._edge_nodes_index_dict[node_id] = self._edge_nodes_index_dict.get(node_id, []) + [way_relation]
-
- def remove(self, way_relation):
- for node in way_relation.way.nodes:
- node_id = node.id
- self._full_nodes_index_dict[node_id] = [wr for wr in self._full_nodes_index_dict.get(node_id, [])
- if wr is not way_relation]
- if node_id in way_relation.edge_nodes_ids:
- self._edge_nodes_index_dict[node_id] = [wr for wr in self._edge_nodes_index_dict.get(node_id, [])
- if wr is not way_relation]
-
- def way_relations_with_edge_node_id(self, node_id):
- return self._edge_nodes_index_dict.get(node_id, [])
-
- def way_relations_with_node_id(self, node_id):
- return self._full_nodes_index_dict.get(node_id, [])
diff --git a/selfdrive/mapd/lib/default_speeds.json b/selfdrive/mapd/lib/default_speeds.json
deleted file mode 100644
index a8db60880..000000000
--- a/selfdrive/mapd/lib/default_speeds.json
+++ /dev/null
@@ -1,111 +0,0 @@
-{
- "_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped",
- "AR:urban": "40",
- "AR:urban:primary": "60",
- "AR:urban:secondary": "60",
- "AR:rural": "110",
- "AT:urban": "50",
- "AT:rural": "100",
- "AT:trunk": "100",
- "AT:motorway": "130",
- "BE:urban": "50",
- "BE-VLG:rural": "70",
- "BE-WAL:rural": "90",
- "BE:trunk": "120",
- "BE:motorway": "120",
- "CH:urban[1]": "50",
- "CH:rural": "80",
- "CH:trunk": "100",
- "CH:motorway": "120",
- "CZ:pedestrian_zone": "20",
- "CZ:living_street": "20",
- "CZ:urban": "50",
- "CZ:urban_trunk": "80",
- "CZ:urban_motorway": "80",
- "CZ:rural": "90",
- "CZ:trunk": "110",
- "CZ:motorway": "130",
- "DK:urban": "50",
- "DK:rural": "80",
- "DK:motorway": "130",
- "DE:living_street": "7",
- "DE:residential": "30",
- "DE:urban": "50",
- "DE:rural": "100",
- "DE:trunk": "none",
- "DE:motorway": "none",
- "FI:urban": "50",
- "FI:rural": "80",
- "FI:trunk": "100",
- "FI:motorway": "120",
- "FR:urban": "50",
- "FR:rural": "80",
- "FR:trunk": "110",
- "FR:motorway": "130",
- "GR:urban": "50",
- "GR:rural": "90",
- "GR:trunk": "110",
- "GR:motorway": "130",
- "HU:urban": "50",
- "HU:rural": "90",
- "HU:trunk": "110",
- "HU:motorway": "130",
- "IT:urban": "50",
- "IT:rural": "90",
- "IT:trunk": "110",
- "IT:motorway": "130",
- "JP:national": "60",
- "JP:motorway": "100",
- "LT:living_street": "20",
- "LT:urban": "50",
- "LT:rural": "90",
- "LT:trunk": "120",
- "LT:motorway": "130",
- "PL:living_street": "20",
- "PL:urban": "50",
- "PL:rural": "90",
- "PL:trunk": "100",
- "PL:motorway": "140",
- "RO:urban": "50",
- "RO:rural": "90",
- "RO:trunk": "100",
- "RO:motorway": "130",
- "RU:living_street": "20",
- "RU:urban": "60",
- "RU:rural": "90",
- "RU:motorway": "110",
- "SK:urban": "50",
- "SK:rural": "90",
- "SK:trunk": "90",
- "SK:motorway": "90",
- "SI:urban": "50",
- "SI:rural": "90",
- "SI:trunk": "110",
- "SI:motorway": "130",
- "ES:living_street": "20",
- "ES:urban": "50",
- "ES:rural": "50",
- "ES:trunk": "90",
- "ES:motorway": "120",
- "SE:urban": "50",
- "SE:rural": "70",
- "SE:trunk": "90",
- "SE:motorway": "110",
- "GB:nsl_restricted": "30 mph",
- "GB:nsl_single": "60 mph",
- "GB:nsl_dual": "70 mph",
- "GB:motorway": "70 mph",
- "UA:urban": "50",
- "UA:rural": "90",
- "UA:trunk": "110",
- "UA:motorway": "130",
- "UZ:living_street": "30",
- "UZ:urban": "70",
- "UZ:rural": "100",
- "UZ:motorway": "110",
- "ZA:trunk": "120",
- "ZA:residential": "60",
- "ZA:rural": "100",
- "ZA:urban": "60",
- "ZA:motorway": "120"
-}
diff --git a/selfdrive/mapd/lib/geo.py b/selfdrive/mapd/lib/geo.py
deleted file mode 100644
index 8f6232225..000000000
--- a/selfdrive/mapd/lib/geo.py
+++ /dev/null
@@ -1,66 +0,0 @@
-from enum import Enum
-import numpy as np
-
-
-R = 6373000.0 # approximate radius of earth in mts
-
-
-def vectors(points):
- """Provides a array of vectors on cartesian space (x, y).
- Each vector represents the path from a point in `points` to the next.
- `points` must by a (N, 2) array of [lat, lon] pairs in radians.
- """
- latA = points[:-1, 0]
- latB = points[1:, 0]
- delta = np.diff(points, axis=0)
- dlon = delta[:, 1]
-
- x = np.sin(dlon) * np.cos(latB)
- y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon))
-
- return np.column_stack((x, y))
-
-
-def ref_vectors(ref, points):
- """Provides a array of vectors on cartesian space (x, y).
- Each vector represents the path from ref to a point in `points`.
- `points` must by a (N, 2) array of [lat, lon] pairs in radians.
- """
- latA = ref[0]
- latB = points[:, 0]
- delta = points - ref
- dlon = delta[:, 1]
-
- x = np.sin(dlon) * np.cos(latB)
- y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon))
-
- return np.column_stack((x, y))
-
-
-def bearing_to_points(point, points):
- """Calculate the bearings (angle from true north clockwise) of the vectors between `point` and each
- one of the entries in `points`. Both `point` and `points` elements are 2 element arrays containing a latitud,
- longitude pair in radians.
- """
- delta = points - point
- x = np.sin(delta[:, 1]) * np.cos(points[:, 0])
- y = np.cos(point[0]) * np.sin(points[:, 0]) - (np.sin(point[0]) * np.cos(points[:, 0]) * np.cos(delta[:, 1]))
- return np.arctan2(x, y)
-
-
-def distance_to_points(point, points):
- """Calculate the distance of the vectors between `point` and each one of the entries in `points`.
- Both `point` and `points` elements are 2 element arrays containing a latitud, longitude pair in radians.
- """
- delta = points - point
- a = np.sin(delta[:, 0] / 2)**2 + np.cos(point[0]) * np.cos(points[:, 0]) * np.sin(delta[:, 1] / 2)**2
- c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a))
- return c * R
-
-
-class DIRECTION(Enum):
- NONE = 0
- AHEAD = 1
- BEHIND = 2
- FORWARD = 3
- BACKWARD = 4
diff --git a/selfdrive/mapd/lib/osm.py b/selfdrive/mapd/lib/osm.py
deleted file mode 100644
index 2fd8f4da4..000000000
--- a/selfdrive/mapd/lib/osm.py
+++ /dev/null
@@ -1,80 +0,0 @@
-import overpy
-import subprocess
-import numpy as np
-from selfdrive.mapd.lib.geo import R
-import os
-from selfdrive.mapd.config import QUERY_RADIUS
-
-OSM_QUERY = ["/data/media/0/osm/bin/osm3s_query", "--db-dir=/data/media/0/osm/db/"]
-
-def create_way(way_id, node_ids, from_way):
- """
- Creates and OSM Way with the given `way_id` and list of `node_ids`, copying attributes and tags from `from_way`
- """
- return overpy.Way(way_id, node_ids=node_ids, attributes={}, result=from_way._result,
- tags=from_way.tags)
-
-
-class OSM():
- def __init__(self, last_gps_pos):
- self.api = overpy.Overpass()
- # self.api = overpy.Overpass(url='http://3.65.170.21/api/interpreter')
-
- self.local_osm_query_fail_count = 0
- self.ways = []
-
- self.last_gps_pos = last_gps_pos
-
- if os.path.isdir("/data/media/0/osm/bin/") and os.path.isdir("/data/media/0/osm/db/"):
- self.local_osm_enabled = True
- print("Local OSM installed")
- try:
- print("Testing local OSM...")
- if len(last_gps_pos) > 0:
- ways = self.fetch_road_ways_around_location(last_gps_pos['latitude'], last_gps_pos['longitude'], QUERY_RADIUS)
- self.local_osm_enabled = len(ways) > 0
- if self.local_osm_enabled:
- self.ways = ways
-
- except Exception as e:
- self.local_osm_enabled = False
- print(f"Local OSM test failed:\n{e}")
- else:
- self.local_osm_enabled = False
- print("Local OSM enabled = %s" % self.local_osm_enabled)
-
- def fetch_road_ways_around_location(self, lat, lon, radius):
- # when lat/lon is same as last_gps_pos lat, lon, we can just use cached way data
- # this only happened when we are using lastGPSPosition params
- if len(self.ways) > 0 and len(self.last_gps_pos) > 0 and self.last_gps_pos["latitude"] == lat and self.last_gps_pos["longitude"] == lon:
- return self.ways
- # Calculate the bounding box coordinates for the bbox containing the circle around location.
- bbox_angle = np.degrees(radius / R)
- # fetch all ways and nodes on this ways in bbox
- bbox_str = f'{str(lat - bbox_angle)},{str(lon - bbox_angle)},{str(lat + bbox_angle)},{str(lon + bbox_angle)}'
- q = """
- way(""" + bbox_str + """)
- [highway]
- [highway!~"^(footway|path|corridor|bridleway|steps|cycleway|construction|bus_guideway|escape|service|track)$"];
- (._;>;);
- out;
- """
- if self.local_osm_enabled:
- try:
- completion = subprocess.run(OSM_QUERY + [f"--request={q}"], check=True, capture_output=True)
- self.ways = self.api.parse_xml(completion.stdout).ways
-
- except Exception as e:
- self.local_osm_query_fail_count += 1
- print(f'Exception while querying local OSM:\n{e}')
- pass
-
- # use remote OSM when local osm is not enabled or failed too many times
- if not self.local_osm_enabled or self.local_osm_query_fail_count >= 5:
- try:
- self.ways = self.api.query(q).ways
- self.local_osm_query_fail_count = 0
- except Exception as e:
- print(f'Exception while querying OSM:\n{e}')
-
- return self.ways
diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
deleted file mode 100644
index 3d240c8b2..000000000
--- a/selfdrive/mapd/mapd.py
+++ /dev/null
@@ -1,313 +0,0 @@
-#!/usr/bin/env python3
-import threading
-from traceback import print_exception
-import numpy as np
-# from time import strftime, gmtime
-import cereal.messaging as messaging
-from common.realtime import Ratekeeper, set_core_affinity, set_realtime_priority
-from selfdrive.mapd.lib.osm import OSM
-from selfdrive.mapd.lib.geo import distance_to_points
-from selfdrive.mapd.lib.WayCollection import WayCollection
-from selfdrive.mapd.config import QUERY_RADIUS, MIN_DISTANCE_FOR_NEW_QUERY, FULL_STOP_MAX_SPEED, LOOK_AHEAD_HORIZON_TIME
-from system.swaglog import cloudlog
-from common.params import Params
-import json
-from cereal import log
-import math
-
-
-_DEBUG = False
-_CLOUDLOG_DEBUG = True
-
-
-def _debug(msg, log_to_cloud=True):
- if _CLOUDLOG_DEBUG and log_to_cloud:
- cloudlog.debug(msg)
- if _DEBUG:
- print(msg)
-
-
-def excepthook(args):
- _debug(f'MapD: Threading exception:\n{args}')
- print_exception(args.exc_type, args.exc_value, args.exc_traceback)
-
-
-threading.excepthook = excepthook
-
-
-class MapD():
- def __init__(self):
- last_gps_params = Params().get('LastGPSPosition')
- self.last_gps_pos = json.loads(last_gps_params) if last_gps_params is not None else []
-
- self.osm = OSM(self.last_gps_pos)
- self.way_collection = None
- self.route = None
- self.last_gps_fix_timestamp = 0
- self.last_gps = None
- self.location_deg = None # The current location in degrees.
- self.location_rad = None # The current location in radians as a Numpy array.
- self.bearing_rad = None
- self.location_stdev = None # The current location accuracy in mts. 1 standard devitation.
- self.gps_speed = 0.
- self.last_fetch_location = None
- self.last_route_update_fix_timestamp = 0
- self.last_publish_fix_timestamp = 0
- # self._op_enabled = False
- self._disengaging = False
- self._query_thread = None
- self._lock = threading.RLock()
- self._location_valid = False
-
- # def udpate_state(self, sm):
- # sock = 'controlsState'
- # if not sm.updated[sock] or not sm.valid[sock]:
- # return
- #
- # controls_state = sm[sock]
- # self._disengaging = not controls_state.enabled and self._op_enabled
- # self._op_enabled = controls_state.enabled
-
- def apply_last_gps_pos(self):
- # use last gps position before first fixed
- if not self._location_valid and self.last_gps_pos is not None and len(self.last_gps_pos) > 0:
- self.location_rad = np.radians(np.array([self.last_gps_pos["latitude"], self.last_gps_pos["longitude"]], dtype=float))
- self.location_deg = (self.last_gps_pos["latitude"], self.last_gps_pos["longitude"])
- self.bearing_rad = np.radians(0., dtype=float)
- self.location_stdev = 5.
-
- def update_car_state(self, sm):
- sock = 'carState'
- if not sm.updated[sock] or not sm.valid[sock]:
- return
- car_state = sm[sock]
- self.gps_speed = car_state.vEgo
- self._disengaging = abs(car_state.steeringRateDeg) > 60
-
- def update_locationd(self, sm):
- sock = 'liveLocationKalman'
- if not sm.updated[sock] or not sm.valid[sock]:
- return
-
- location = sm[sock]
- self._location_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
-
- if self._location_valid:
- self.last_gps_fix_timestamp = location.unixTimestampMillis
- self.location_stdev = 1.25
-
- lat = location.positionGeodetic.value[0]
- lon = location.positionGeodetic.value[1]
- self.location_rad = np.radians(np.array([lat, lon], dtype=float))
- self.location_deg = (lat, lon)
- self.bearing_rad = np.radians(math.degrees(location.calibratedOrientationNED.value[2]), dtype=float)
-
- # def update_gps(self, sm):
- # sock = 'gpsLocationExternal'
- # if not sm.updated[sock] or not sm.valid[sock]:
- # return
- #
- # log = sm[sock]
- # self.last_gps = log
- #
- # # ignore the message if the fix is invalid
- # if log.flags % 2 == 0:
- # return
- #
- # self.last_gps_fix_timestamp = log.unixTimestampMillis # Unix TS. Milliseconds since January 1, 1970.
- # self.location_rad = np.radians(np.array([log.latitude, log.longitude], dtype=float))
- # self.location_deg = (log.latitude, log.longitude)
- # self.bearing_rad = np.radians(log.bearingDeg, dtype=float)
- # self.gps_speed = log.speed
- # self.location_stdev = log.accuracy # log accuracies are presumably 1 standard deviation.
- #
- # _debug('Mapd: ********* Got GPS fix'
- # + f'Pos: {self.location_deg} +/- {self.location_stdev * 2.} mts.\n'
- # + f'Bearing: {log.bearingDeg} +/- {log.bearingAccuracyDeg * 2.} deg.\n'
- # + f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}'
- # + '*******', log_to_cloud=False)
-
- def _query_osm_not_blocking(self):
- def query(osm, location_deg, location_rad, radius):
- _debug(f'Mapd: Start query for OSM map data at {location_deg}')
- lat, lon = location_deg
- ways = osm.fetch_road_ways_around_location(lat, lon, radius)
- _debug(f'Mapd: Query to OSM finished with {len(ways)} ways')
-
- # Only issue an update if we received some ways. Otherwise it is most likely a conectivity issue.
- # Will retry on next loop.
- if len(ways) > 0:
- new_way_collection = WayCollection(ways, location_rad)
-
- # Use the lock to update the way_collection as it might be being used to update the route.
- _debug('Mapd: Locking to write results from osm.', log_to_cloud=False)
- with self._lock:
- self.way_collection = new_way_collection
- self.last_fetch_location = location_rad
- _debug(f'Mapd: Updated map data @ {location_deg} - got {len(ways)} ways')
-
- _debug('Mapd: Releasing Lock to write results from osm', log_to_cloud=False)
-
- # Ignore if we have a query thread already running.
- if self._query_thread is not None and self._query_thread.is_alive():
- return
-
- self._query_thread = threading.Thread(target=query, args=(self.osm, self.location_deg, self.location_rad,
- QUERY_RADIUS))
- self._query_thread.start()
-
- def updated_osm_data(self):
- if self.route is not None:
- distance_to_end = self.route.distance_to_end
- if distance_to_end is not None and distance_to_end >= MIN_DISTANCE_FOR_NEW_QUERY:
- # do not query as long as we have a route with enough distance ahead.
- return
-
- if self.location_rad is None:
- return
-
- if self.last_fetch_location is not None:
- distance_since_last = distance_to_points(self.last_fetch_location, np.array([self.location_rad]))[0]
- if distance_since_last < QUERY_RADIUS - MIN_DISTANCE_FOR_NEW_QUERY:
- # do not query if are still not close to the border of previous query area
- return
-
- self._query_osm_not_blocking()
-
- def update_route(self):
- def update_proc():
- # Ensure we clear the route on op disengage, this way we can correct possible incorrect map data due
- # to wrongly locating or picking up the wrong route.
- if self._disengaging:
- self.route = None
- _debug('Mapd *****: Clearing Route as system is disengaging. ********')
-
- if self.way_collection is None or self.location_rad is None or self.bearing_rad is None:
- _debug('Mapd *****: Can not update route. Missing WayCollection, location or bearing ********')
- return
-
- if self.route is not None and self.last_route_update_fix_timestamp == self.last_gps_fix_timestamp:
- _debug('Mapd *****: Skipping route update. No new fix since last update ********')
- return
-
- self.last_route_update_fix_timestamp = self.last_gps_fix_timestamp
-
- # Create the route if not existent or if it was generated by an older way collection
- if self.route is None or self.route.way_collection_id != self.way_collection.id:
- self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad, self.location_stdev)
- _debug(f'Mapd *****: Route created: \n{self.route}\n********')
- return
-
- # Do not attempt to update the route if the car is going close to a full stop, as the bearing can start
- # jumping and creating unnecesary loosing of the route. Since the route update timestamp has been updated
- # a new liveMapData message will be published with the current values (which is desirable)
- if self.gps_speed < FULL_STOP_MAX_SPEED:
- _debug('Mapd *****: Route Not updated as car has Stopped ********')
- return
-
- self.route.update(self.location_rad, self.bearing_rad, self.location_stdev)
- if self.route.located:
- _debug(f'Mapd *****: Route updated: \n{self.route}\n********')
- return
-
- # if an old route did not mange to locate, attempt to regenerate form way collection.
- self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad, self.location_stdev)
- _debug(f'Mapd *****: Failed to update location in route. Regenerated with route: \n{self.route}\n********')
-
- # We use the lock when updating the route, as it reads `way_collection` which can ben updated by
- # a new query result from the _query_thread.
- _debug('Mapd: Locking to update route.', log_to_cloud=False)
- with self._lock:
- update_proc()
-
- _debug('Mapd: Releasing Lock to update route', log_to_cloud=False)
-
- def publish(self, pm, sm):
- # Ensure we have a route currently located
- if self.route is None or not self.route.located:
- _debug('Mapd: Skipping liveMapData message as there is no route or is not located.')
- return
-
- # Ensure we have a route update since last publish
- if self.last_publish_fix_timestamp == self.last_route_update_fix_timestamp:
- _debug('Mapd: Skipping liveMapData since there is no new gps fix.')
- return
-
- self.last_publish_fix_timestamp = self.last_route_update_fix_timestamp
-
- speed_limit = self.route.current_speed_limit
- next_speed_limit_section = self.route.next_speed_limit_section
- # turn_speed_limit_section = self.route.current_curvature_speed_limit_section
- # horizon_mts = self.gps_speed * LOOK_AHEAD_HORIZON_TIME
- # next_turn_speed_limit_sections = self.route.next_curvature_speed_limit_sections(horizon_mts)
- current_road_name = self.route.current_road_name
-
- map_data_msg = messaging.new_message('liveMapData')
- map_data_msg.valid = sm.all_alive(service_list=['liveLocationKalman']) and \
- sm.all_valid(service_list=['liveLocationKalman'])
-
- # map_data_msg.liveMapData.lastGpsTimestamp = self.last_gps.unixTimestampMillis
- # map_data_msg.liveMapData.lastGpsLatitude = float(self.last_gps.latitude)
- # map_data_msg.liveMapData.lastGpsLongitude = float(self.last_gps.longitude)
- # map_data_msg.liveMapData.lastGpsSpeed = float(self.last_gps.speed)
- # map_data_msg.liveMapData.lastGpsBearingDeg = float(self.last_gps.bearingDeg)
- # map_data_msg.liveMapData.lastGpsAccuracy = float(self.last_gps.accuracy)
- # map_data_msg.liveMapData.lastGpsBearingAccuracyDeg = float(self.last_gps.bearingAccuracyDeg)
-
- map_data_msg.liveMapData.speedLimitValid = bool(speed_limit is not None)
- map_data_msg.liveMapData.speedLimit = float(speed_limit if speed_limit is not None else 0.0)
- map_data_msg.liveMapData.speedLimitAheadValid = bool(next_speed_limit_section is not None)
- map_data_msg.liveMapData.speedLimitAhead = float(next_speed_limit_section.value
- if next_speed_limit_section is not None else 0.0)
- map_data_msg.liveMapData.speedLimitAheadDistance = float(next_speed_limit_section.start
- if next_speed_limit_section is not None else 0.0)
-
- # map_data_msg.liveMapData.turnSpeedLimitValid = bool(turn_speed_limit_section is not None)
- # map_data_msg.liveMapData.turnSpeedLimit = float(turn_speed_limit_section.value
- # if turn_speed_limit_section is not None else 0.0)
- # map_data_msg.liveMapData.turnSpeedLimitSign = int(turn_speed_limit_section.curv_sign
- # if turn_speed_limit_section is not None else 0)
- # map_data_msg.liveMapData.turnSpeedLimitEndDistance = float(turn_speed_limit_section.end
- # if turn_speed_limit_section is not None else 0.0)
- # map_data_msg.liveMapData.turnSpeedLimitsAhead = [float(s.value) for s in next_turn_speed_limit_sections]
- # map_data_msg.liveMapData.turnSpeedLimitsAheadDistances = [float(s.start) for s in next_turn_speed_limit_sections]
- # map_data_msg.liveMapData.turnSpeedLimitsAheadSigns = [float(s.curv_sign) for s in next_turn_speed_limit_sections]
-
- map_data_msg.liveMapData.currentRoadName = str(current_road_name if current_road_name is not None else "")
-
- pm.send('liveMapData', map_data_msg)
- _debug(f'Mapd *****: Publish: \n{map_data_msg}\n********', log_to_cloud=False)
-
-
-# provides live map data information
-def mapd_thread(sm=None, pm=None):
- set_core_affinity([1,])
- set_realtime_priority(1)
- mapd = MapD()
- rk = Ratekeeper(1., print_delay_threshold=None) # Keeps rate at 1 hz
-
- # *** setup messaging
- if sm is None:
- sm = messaging.SubMaster(["carState", "liveLocationKalman"])
- if pm is None:
- pm = messaging.PubMaster(['liveMapData'])
-
- while True:
- sm.update()
- mapd.apply_last_gps_pos()
- # mapd.udpate_state(sm)
- mapd.update_car_state(sm)
- mapd.update_locationd(sm)
- # mapd.update_gps(sm)
- mapd.updated_osm_data()
- mapd.update_route()
- mapd.publish(pm, sm)
- rk.keep_time()
-
-
-def main(sm=None, pm=None):
- mapd_thread(sm, pm)
-
-
-if __name__ == "__main__":
- main()
diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py
deleted file mode 100644
index 58a33ee41..000000000
--- a/selfdrive/mapd/mapd_helpers.py
+++ /dev/null
@@ -1,364 +0,0 @@
-import math
-import json
-import numpy as np
-from datetime import datetime
-from common.basedir import BASEDIR
-from selfdrive.config import Conversions as CV
-from common.transformations.coordinates import LocalCoord, geodetic2ecef
-
-LOOKAHEAD_TIME = 10.
-MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME
-
-DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json"
-DEFAULT_SPEEDS = {}
-with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f:
- DEFAULT_SPEEDS = json.loads(f.read())
-
-DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json"
-DEFAULT_SPEEDS_BY_REGION = {}
-with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f:
- DEFAULT_SPEEDS_BY_REGION = json.loads(f.read())
-
-def circle_through_points(p1, p2, p3):
- """Fits a circle through three points
- Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm"""
- x1, y1, _ = p1
- x2, y2, _ = p2
- x3, y3, _ = p3
-
- A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2
- B = (x1**2 + y1**2) * (y3 - y2) + (x2**2 + y2**2) * (y1 - y3) + (x3**2 + y3**2) * (y2 - y1)
- C = (x1**2 + y1**2) * (x2 - x3) + (x2**2 + y2**2) * (x3 - x1) + (x3**2 + y3**2) * (x1 - x2)
- D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2)
-
- return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2)))
-
-def parse_speed_unit(max_speed):
- """Converts a maxspeed string to m/s based on the unit present in the input.
- OpenStreetMap defaults to kph if no unit is present. """
-
- if not max_speed:
- return None
-
- conversion = CV.KPH_TO_MS
- if 'mph' in max_speed:
- max_speed = max_speed.replace(' mph', '')
- conversion = CV.MPH_TO_MS
- try:
- return float(max_speed) * conversion
- except ValueError:
- return None
-
-def parse_speed_tags(tags):
- """Parses tags on a way to find the maxspeed string"""
- max_speed = None
-
- if 'maxspeed' in tags:
- max_speed = tags['maxspeed']
-
- if 'maxspeed:conditional' in tags:
- try:
- max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ')
- cond = cond[1:-1]
-
- start, end = cond.split('-')
- now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays
- start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
- end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
-
- if start <= now <= end:
- max_speed = max_speed_cond
- except ValueError:
- pass
-
- if not max_speed and 'source:maxspeed' in tags:
- max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None)
- if not max_speed and 'maxspeed:type' in tags:
- max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None)
-
- max_speed = parse_speed_unit(max_speed)
- return max_speed
-
-def geocode_maxspeed(tags, location_info):
- max_speed = None
- try:
- geocode_country = location_info.get('country', '')
- geocode_region = location_info.get('region', '')
-
- country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {})
- country_defaults = country_rules.get('Default', [])
- for rule in country_defaults:
- rule_valid = all(
- tag_name in tags
- and tags[tag_name] == value
- for tag_name, value in rule['tags'].items()
- )
- if rule_valid:
- max_speed = rule['speed']
- break #stop searching country
-
- region_rules = country_rules.get(geocode_region, [])
- for rule in region_rules:
- rule_valid = all(
- tag_name in tags
- and tags[tag_name] == value
- for tag_name, value in rule['tags'].items()
- )
- if rule_valid:
- max_speed = rule['speed']
- break #stop searching region
- except KeyError:
- pass
- max_speed = parse_speed_unit(max_speed)
- return max_speed
-
-class Way:
- def __init__(self, way, query_results):
- self.id = way.id
- self.way = way
- self.query_results = query_results
-
- points = list()
-
- for node in self.way.get_nodes(resolve_missing=False):
- points.append((float(node.lat), float(node.lon), 0.))
-
- self.points = np.asarray(points)
-
- @classmethod
- def closest(cls, query_results, lat, lon, heading, prev_way=None):
- results, tree, real_nodes, node_to_way, location_info = query_results
-
- cur_pos = geodetic2ecef((lat, lon, 0))
- nodes = tree.query_ball_point(cur_pos, 500)
-
- # If no nodes within 500m, choose closest one
- if not nodes:
- nodes = [tree.query(cur_pos)[1]]
-
- ways = []
- for n in nodes:
- real_node = real_nodes[n]
- ways += node_to_way[real_node.id]
- ways = set(ways)
-
- closest_way = None
- best_score = None
- for way in ways:
- way = Way(way, query_results)
- points = way.points_in_car_frame(lat, lon, heading)
-
- on_way = way.on_way(lat, lon, heading, points)
- if not on_way:
- continue
-
- # Create mask of points in front and behind
- x = points[:, 0]
- y = points[:, 1]
- angles = np.arctan2(y, x)
- front = np.logical_and((-np.pi / 2) < angles,
- angles < (np.pi / 2))
- behind = np.logical_not(front)
-
- dists = np.linalg.norm(points, axis=1)
-
- # Get closest point behind the car
- dists_behind = np.copy(dists)
- dists_behind[front] = np.NaN
- closest_behind = points[np.nanargmin(dists_behind)]
-
- # Get closest point in front of the car
- dists_front = np.copy(dists)
- dists_front[behind] = np.NaN
- closest_front = points[np.nanargmin(dists_front)]
-
- # fit line: y = a*x + b
- x1, y1, _ = closest_behind
- x2, y2, _ = closest_front
- a = (y2 - y1) / max((x2 - x1), 1e-5)
- b = y1 - a * x1
-
- # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error
- # (A 20 degree heading offset results in an a of about 1/3)
- score = abs(a) * 60. + abs(b)
-
- # Prefer same type of road
- if prev_way is not None:
- if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''):
- score *= 0.5
-
- if closest_way is None or score < best_score:
- closest_way = way
- best_score = score
-
- # Normal score is < 5
- if best_score > 50:
- return None
-
- return closest_way
-
- def __str__(self):
- return "%s %s" % (self.id, self.way.tags)
-
- def max_speed(self):
- """Extracts the (conditional) speed limit from a way"""
- if not self.way:
- return None
-
- max_speed = parse_speed_tags(self.way.tags)
- if not max_speed:
- location_info = self.query_results[4]
- max_speed = geocode_maxspeed(self.way.tags, location_info)
-
- return max_speed
-
- def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
- """Look ahead for a max speed"""
- if not self.way:
- return None
-
- speed_ahead = None
- speed_ahead_dist = None
- lookahead_ways = 5
- way = self
- for i in range(lookahead_ways):
- way_pts = way.points_in_car_frame(lat, lon, heading)
-
- # Check current lookahead distance
- max_dist = np.linalg.norm(way_pts[-1, :])
-
- if max_dist > 2 * lookahead:
- break
-
- if 'maxspeed' in way.way.tags:
- spd = parse_speed_tags(way.way.tags)
- if not spd:
- location_info = self.query_results[4]
- spd = geocode_maxspeed(way.way.tags, location_info)
- if spd < current_speed_limit:
- speed_ahead = spd
- min_dist = np.linalg.norm(way_pts[1, :])
- speed_ahead_dist = min_dist
- break
- # Find next way
- way = way.next_way()
- if not way:
- break
-
- return speed_ahead, speed_ahead_dist
-
- def advisory_max_speed(self):
- if not self.way:
- return None
-
- tags = self.way.tags
- adv_speed = None
-
- if 'maxspeed:advisory' in tags:
- adv_speed = tags['maxspeed:advisory']
- adv_speed = parse_speed_unit(adv_speed)
- return adv_speed
-
- def on_way(self, lat, lon, heading, points=None):
- if points is None:
- points = self.points_in_car_frame(lat, lon, heading)
- x = points[:, 0]
- return np.min(x) < 0. and np.max(x) > 0.
-
- def closest_point(self, lat, lon, heading, points=None):
- if points is None:
- points = self.points_in_car_frame(lat, lon, heading)
- i = np.argmin(np.linalg.norm(points, axis=1))
- return points[i]
-
- def distance_to_closest_node(self, lat, lon, heading, points=None):
- if points is None:
- points = self.points_in_car_frame(lat, lon, heading)
- return np.min(np.linalg.norm(points, axis=1))
-
- def points_in_car_frame(self, lat, lon, heading):
- lc = LocalCoord.from_geodetic([lat, lon, 0.])
-
- # Build rotation matrix
- heading = math.radians(-heading + 90)
- c, s = np.cos(heading), np.sin(heading)
- rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]])
-
- # Convert to local coordinates
- points_carframe = lc.geodetic2ned(self.points).T
-
- # Rotate with heading of car
- points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T
-
- return points_carframe
-
- def next_way(self, backwards=False):
- results, tree, real_nodes, node_to_way, location_info = self.query_results
-
- if backwards:
- node = self.way.nodes[0]
- else:
- node = self.way.nodes[-1]
-
- ways = node_to_way[node.id]
-
- way = None
- try:
- # Simple heuristic to find next way
- ways = [w for w in ways if w.id != self.id]
- ways = [w for w in ways if w.nodes[0] == node]
-
- # Filter on highway tag
- acceptable_tags = list()
- cur_tag = self.way.tags['highway']
- acceptable_tags.append(cur_tag)
- if cur_tag == 'motorway_link':
- acceptable_tags.append('motorway')
- acceptable_tags.append('trunk')
- acceptable_tags.append('primary')
- ways = [w for w in ways if w.tags['highway'] in acceptable_tags]
-
- # Filter on number of lanes
- cur_num_lanes = int(self.way.tags['lanes'])
- if len(ways) > 1:
- ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes]
- if len(ways_same_lanes) == 1:
- ways = ways_same_lanes
- if len(ways) > 1:
- ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes]
- if len(ways) == 1:
- way = Way(ways[0], self.query_results)
-
- except (KeyError, ValueError):
- pass
-
- return way
-
- def get_lookahead(self, lat, lon, heading, lookahead):
- pnts = None
- way = self
- valid = False
-
- for i in range(5):
- # Get new points and append to list
- new_pnts = way.points_in_car_frame(lat, lon, heading)
-
- if pnts is None:
- pnts = new_pnts
- else:
- pnts = np.vstack([pnts, new_pnts])
-
- # Check current lookahead distance
- max_dist = np.linalg.norm(pnts[-1, :])
- if max_dist > lookahead:
- valid = True
-
- if max_dist > 2 * lookahead:
- break
-
- # Find next way
- way = way.next_way()
- if not way:
- break
-
- return pnts, valid
diff --git a/selfdrive/mapd/test/__init__.py b/selfdrive/mapd/test/__init__.py
deleted file mode 100644
index e69de29bb..000000000
diff --git a/selfdrive/mapd/test/mock_data.py b/selfdrive/mapd/test/mock_data.py
deleted file mode 100644
index b7da229bd..000000000
--- a/selfdrive/mapd/test/mock_data.py
+++ /dev/null
@@ -1,266 +0,0 @@
-from selfdrive.mapd.lib.WayCollection import WayCollection
-from selfdrive.mapd.lib.geo import vectors, R
-from selfdrive.mapd.lib.NodesData import _MIN_NODE_DISTANCE, _ADDED_NODES_DIST, _SPLINE_EVAL_STEP, \
- _MIN_SPEED_SECTION_LENGTH, nodes_raw_data_array_for_wr, node_calculations, is_wr_a_valid_divertion_from_node, \
- spline_curvature_calculations, speed_limits_for_curvatures_data
-from scipy.interpolate import splev, splprep
-import numpy as np
-import overpy
-
-
-class MockNodesData():
- def __init__(self, way_coords):
- self.degrees = np.array(way_coords)
- self.radians = np.radians(self.degrees)
-
- # *****************
- # Expected code implementation nodes_data
- self.v = vectors(self.radians) * R
- self.d = np.linalg.norm(self.v, axis=1)
- self.b = np.arctan2(self.v[:, 0], self.v[:, 1])
- self.v = np.concatenate(([[0., 0.]], self.v))
- self.dp = np.concatenate(([0.], self.d))
- self.dn = np.concatenate((self.d, [0.]))
- self.dr = np.cumsum(self.dp, axis=0)
- self.b = np.concatenate((self.b, [self.b[-1]]))
-
- # Expected code implementation spline_curvature_calculations
- vect = self.v
- dist_prev = self.dp
- too_far_idxs = np.nonzero(self.dp >= _MIN_NODE_DISTANCE)[0]
- for idx in too_far_idxs[::-1]:
- dp = dist_prev[idx] # distance of vector that needs to be replaced by higher resolution vectors.
- n = int(np.ceil(dp / _ADDED_NODES_DIST)) # number of vectors that need to be added.
- new_v = vect[idx, :] / n # new relative vector to insert.
- vect = np.delete(vect, idx, axis=0) # remove the relative vector to be replaced by the insertion of new vectors.
- vect = np.insert(vect, [idx] * n, [new_v] * n, axis=0) # insert n new relative vectors
- ds = np.cumsum(dist_prev, axis=0)
- vs = np.cumsum(vect, axis=0)
- tck, u = splprep([vs[:, 0], vs[:, 1]]) # pylint: disable=W0632
- n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u))
- unew = np.arange(0, n + 1) / n
- d1 = splev(unew, tck, der=1)
- d2 = splev(unew, tck, der=2)
- num = d1[0] * d2[1] - d1[1] * d2[0]
- den = (d1[0]**2 + d1[1]**2)**(1.5)
- self.curv = num / den
- self.curv_ds = unew * ds[-1]
- # *****************
-
-
-class MockCurveSection():
- def __init__(self, func, di=0., df=1000., step=10.):
- self.di = di
- self.df = df
- self.n = (df - di) // step
- self.u = np.arange(0, self.n + 1) / self.n
- self.curv_ds = self.u * (df - di) + di
- self.curv = func(self.u)
- self.curv_abs = np.abs(self.curv)
- self.curv_sec = np.column_stack((self.curv_abs, np.sign(self.curv), self.curv_ds))
-
-
-class MockOSMQueryResponse():
- def __init__(self, xml_path, query_center):
- self.api = overpy.Overpass()
- self.query_center = np.radians(np.array(query_center))
-
- with open(xml_path, 'r') as f:
- overpass_xml = f.read()
- self.ways = self.api.parse_xml(overpass_xml).ways
-
- self.wayCollection = WayCollection(self.ways, self.query_center)
-
-class MockRouteData():
- def __init__(self, way_ids, way_collection, first_node_id): # way)ids must be in order forming a route.
- self.wrs = [next(wr for wr in way_collection.way_relations if wr.id == way_id) for way_id in way_ids]
- self.way_collection = way_collection
- self.first_node_id = first_node_id
-
- def reset(self):
- way_relations = self.wrs
- wr_index = self.way_collection.wr_index
-
- # Nodes Data processing expects way relations to be updated with direction before running.
- for idx, wr in enumerate(way_relations):
- if idx == 0:
- wr.update_direction_from_starting_node(self.first_node_id)
- else:
- wr.update_direction_from_starting_node(way_relations[idx - 1].last_node.id)
-
- # ***** Expected calculations
- self._nodes_data = np.array([])
- self._divertions = [[]]
- self._curvature_speed_sections_data = np.array([])
- way_count = len(way_relations)
- if way_count == 0:
- return
- # We want all the nodes from the last way section
- nodes_data = nodes_raw_data_array_for_wr(way_relations[-1])
- # For the ways before the last in the route we want all the nodes but the last, as that one is the first on
- # the next section. Collect them, append last way node data and concatenate the numpy arrays.
- if way_count > 1:
- wrs_data = tuple([nodes_raw_data_array_for_wr(wr, drop_last=True) for wr in way_relations[:-1]])
- wrs_data += (nodes_data,)
- nodes_data = np.concatenate(wrs_data)
- # Get a subarray with lat, lon to compute the remaining node values.
- lat_lon_array = nodes_data[:, [1, 2]]
- points = np.radians(lat_lon_array)
- # Ensure we have more than 3 points, if not calculations are not possible.
- if len(points) <= 3:
- return
- vect, dist_prev, dist_next, dist_route, bearing = node_calculations(points)
- # append calculations to nodes_data
- # nodes_data structure: [id, lat, lon, speed_limit, x, y, dist_prev, dist_next, dist_route, bearing]
- self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, dist_route, bearing))
- # Build route divertion options data from the wr_index.
- wr_ids = [wr.id for wr in way_relations]
- self._divertions = [[wr for wr in wr_index.way_relations_with_edge_node_id(node_id)
- if is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids)]
- for node_id in nodes_data[:, 0]]
- # Store calculcations for curvature sections speed limits. We need more than 3 points to be able to process.
- # _curvature_speed_sections_data structure: [dist_start, dist_stop, speed_limits, curv_sign]
- if len(vect) > 3:
- self._curv, self._curv_ds = spline_curvature_calculations(vect, dist_prev)
- self._curvature_speed_sections_data = speed_limits_for_curvatures_data(self._curv, self._curv_ds)
- # *****
-
-
-# Test data in degrees from this road:
-# https://www.google.de/maps/@52.209263,13.8723137,13z
-_WAY_NODES_COORDS_01 = [
- [52.1933703, 13.8723799],
- [52.1939477, 13.8711273],
- [52.1942004, 13.8705818],
- [52.1945408, 13.8698496],
- [52.1948447, 13.8691873],
- [52.1950772, 13.8685726],
- [52.1951168, 13.8684641],
- [52.1956681, 13.8670323],
- [52.1958716, 13.8664936],
- [52.1964366, 13.8649875],
- [52.1969283, 13.8636040],
- [52.1970203, 13.8634430],
- [52.1975486, 13.8626307],
- [52.1976354, 13.8624971],
- [52.1977827, 13.8621795],
- [52.1978564, 13.8619220],
- [52.1981843, 13.8604497],
- [52.1982614, 13.8602140],
- [52.1983351, 13.8600595],
- [52.1992768, 13.8579824],
- [52.1995107, 13.8574321],
- [52.1995948, 13.8572604],
- [52.1996818, 13.8571155],
- [52.1998000, 13.8570029],
- [52.2000659, 13.8568236],
- [52.2003868, 13.8566005],
- [52.2007182, 13.8564460],
- [52.2008760, 13.8564117],
- [52.2009865, 13.8564117],
- [52.2011390, 13.8564202],
- [52.2012267, 13.8564496],
- [52.2012544, 13.8564577],
- [52.2013179, 13.8564803],
- [52.2020491, 13.8571756],
- [52.2026014, 13.8576991],
- [52.2027592, 13.8578879],
- [52.2027960, 13.8579309],
- [52.2028960, 13.8580939],
- [52.2030170, 13.8583343],
- [52.2036587, 13.8597076],
- [52.2052946, 13.8633039],
- [52.2064332, 13.8658435],
- [52.2067856, 13.8666332],
- [52.2068961, 13.8668477],
- [52.2070777, 13.8670890],
- [52.2073723, 13.8674409],
- [52.2077457, 13.8679387],
- [52.2083874, 13.8687455],
- [52.2093341, 13.8699214],
- [52.2099652, 13.8707540],
- [52.2102282, 13.8712089],
- [52.2104228, 13.8715694],
- [52.2106122, 13.8718955],
- [52.2107619, 13.8721756],
- [52.2108695, 13.8723771],
- [52.2110747, 13.8727610],
- [52.2111514, 13.8729047],
- [52.2114010, 13.8733718],
- [52.2114694, 13.8735006],
- [52.2115430, 13.8736636],
- [52.2116086, 13.8737571],
- [52.2116770, 13.8738172],
- [52.2117611, 13.8738515],
- [52.2118664, 13.8738566],
- [52.2119322, 13.8738439],
- [52.2121058, 13.8737924],
- [52.2122583, 13.8737495],
- [52.2123265, 13.8737260],
- [52.2124213, 13.8736894],
- [52.2127466, 13.8734888],
- [52.2128263, 13.8734491],
- [52.2131313, 13.8733117],
- [52.2133943, 13.8731830],
- [52.2136625, 13.8731057],
- [52.2139465, 13.8730456],
- [52.2143619, 13.8730113],
- [52.2148773, 13.8729942],
- [52.2152275, 13.8730325],
- [52.2153110, 13.8730398],
- [52.2157442, 13.8730848],
- [52.2158833, 13.8731036]]
-
-
-mockNodesData01 = MockNodesData(_WAY_NODES_COORDS_01)
-
-# OSM Query around B96 south of Berlin
-mockOSMResponse01 = MockOSMQueryResponse('selfdrive/mapd/test/mock_osm_response_01.xml',
- [52.31400353586984, 13.447158941786366])
-
-# OSM Query on curvy town area south of Germany.
-mockOSMResponse02 = MockOSMQueryResponse('selfdrive/mapd/test/mock_osm_response_02.xml',
- [48.16573269276522, 9.81418473659117])
-
-mockWayCollection01 = WayCollection(mockOSMResponse01.ways, mockOSMResponse01.query_center)
-mockWayCollection02 = WayCollection(mockOSMResponse02.ways, mockOSMResponse02.query_center)
-
-# Normal curvy Way. way id: 179532213 with 35 Nodes.
-mockOSMWay_01_01_LongCurvy = next(way for way in mockOSMResponse01.ways if way.id == 179532213)
-
-# Looped way. way id: 29233907
-mockOSMWay_01_02_Loop = next(way for way in mockOSMResponse01.ways if way.id == 29233907)
-
-# Complex curvy road through town with intersections. way id:178450395
-mockOSMWay_02_01_CurvyTownWithIntersections = next(way for way in mockOSMResponse02.ways if way.id == 178450395)
-
-# Valid divertion for way 02_01 at node: 34785115. way id: 27955186
-mockOSMWay_02_02_Divertion_34785115 = next(way for way in mockOSMResponse02.ways if way.id == 27955186)
-
-# 3 node way. way id: 807781992
-mockOSMWay_02_03_Short_3_node_way = next(way for way in mockOSMResponse02.ways if way.id == 807781992)
-
-# data composing route 01 in way collection 02
-mockRouteData_02_01 = MockRouteData([60890967, 737120246, 601406617, 60890971, 178450395], mockWayCollection02,
- first_node_id=201962346)
-
-# data composing route 02 in way collection 02. Single WR
-mockRouteData_02_02_single_wr = MockRouteData([178450395], mockWayCollection02, first_node_id=762086638)
-
-# data composing route 03 in way collection 02. Multiple speed limits
-mockRouteData_02_03 = MockRouteData([158799549, 798805532, 28707704, 158797898, 602249535, 602249536, 825823509,
- 178449088, 916462523, 158796386], mockWayCollection02,
- first_node_id=252601829)
-
-# 1000mt section with one full sin cycle as curv values.
-mockCurveSectionSin = MockCurveSection(lambda x: np.sin(x * 2 * np.pi))
-
-# 200mt section with changing curvature rate.
-mockCurveSteepCurvChange = MockCurveSection(lambda x: 0.05 * x**3 - 0.007 * x**2 + 0.001 * x, df=200)
-
-# _MIN_SPEED_SECTION_LENGTH section with changing curvature rate.
-mockCurveSteepCurvChangeShort = MockCurveSection(
- lambda x: 0.05 * x**3 - 0.007 * x**2 + 0.001 * x, df=_MIN_SPEED_SECTION_LENGTH)
-
-# 200mt section with smooth changing curvature rate. no deviation over 2.
-mockCurveSmoothCurveChange = MockCurveSection(lambda x: 0.0002 * x**3 - 0.001 * x**2 + 0.6 * x, df=200)
diff --git a/selfdrive/mapd/test/mock_osm_response_01.xml b/selfdrive/mapd/test/mock_osm_response_01.xml
deleted file mode 100644
index d1e2a219c..000000000
--- a/selfdrive/mapd/test/mock_osm_response_01.xml
+++ /dev/null
@@ -1,9908 +0,0 @@
-
-
-
-The data included in this document is from www.openstreetmap.org. The data is made available under ODbL.
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/selfdrive/mapd/test/mock_osm_response_02.xml b/selfdrive/mapd/test/mock_osm_response_02.xml
deleted file mode 100644
index bbcf72f3e..000000000
--- a/selfdrive/mapd/test/mock_osm_response_02.xml
+++ /dev/null
@@ -1,11529 +0,0 @@
-
-
-
-The data included in this document is from www.openstreetmap.org. The data is made available under ODbL.
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/selfdrive/mapd/test/test_NodesData.py b/selfdrive/mapd/test/test_NodesData.py
deleted file mode 100644
index 915f575ed..000000000
--- a/selfdrive/mapd/test/test_NodesData.py
+++ /dev/null
@@ -1,354 +0,0 @@
-import unittest
-import numpy as np
-from selfdrive.mapd.lib.geo import DIRECTION
-from common.conversions import Conversions as CV
-from selfdrive.mapd.lib.WayRelation import WayRelation
-from selfdrive.mapd.lib.NodesData import nodes_raw_data_array_for_wr, node_calculations, \
- spline_curvature_calculations, split_speed_section_by_sign, split_speed_section_by_curv_degree, speed_section, \
- speed_limits_for_curvatures_data, is_wr_a_valid_divertion_from_node, SpeedLimitSection, TurnSpeedLimitSection, \
- NodesData, NodeDataIdx
-from selfdrive.mapd.test.mock_data import mockOSMWay_01_01_LongCurvy, mockNodesData01, mockCurveSectionSin, \
- mockCurveSteepCurvChange, mockCurveSteepCurvChangeShort, mockCurveSmoothCurveChange, \
- mockOSMWay_02_01_CurvyTownWithIntersections, mockOSMWay_02_02_Divertion_34785115, mockOSMWay_02_03_Short_3_node_way, \
- mockRouteData_02_01, mockRouteData_02_02_single_wr, mockRouteData_02_03
-from numpy.testing import assert_array_almost_equal
-
-
-class TestNodesDataFileFunctions(unittest.TestCase):
- def test_nodes_raw_data_array_for_wr(self):
- wr = WayRelation(mockOSMWay_01_01_LongCurvy)
- data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float)
- data = nodes_raw_data_array_for_wr(wr)
-
- assert_array_almost_equal(data, data_e)
-
- def test_nodes_raw_data_array_for_wr_flips_when_backwards(self):
- wr = WayRelation(mockOSMWay_01_01_LongCurvy)
- wr.direction = DIRECTION.BACKWARD
-
- data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float)
- data_e = np.flip(data_e, axis=0)
-
- data = nodes_raw_data_array_for_wr(wr)
-
- assert_array_almost_equal(data, data_e)
-
- def test_nodes_raw_data_array_for_wr_drops_last(self):
- wr = WayRelation(mockOSMWay_01_01_LongCurvy)
- data_e = np.array([(n.id, n.lat, n.lon, wr.speed_limit) for n in wr.way.nodes], dtype=float)[:-1]
- data = nodes_raw_data_array_for_wr(wr, drop_last=True)
-
- assert_array_almost_equal(data, data_e)
-
- def test_node_calculations(self):
- points = mockNodesData01.radians
-
- v, dp, dn, dr, b = node_calculations(points)
-
- assert_array_almost_equal(v, mockNodesData01.v)
- assert_array_almost_equal(dp, mockNodesData01.dp)
- assert_array_almost_equal(dn, mockNodesData01.dn)
- assert_array_almost_equal(dr, mockNodesData01.dr)
- assert_array_almost_equal(b, mockNodesData01.b)
-
- def test_node_calculations_index_error(self):
- points = mockNodesData01.radians[:2]
-
- with self.assertRaises(IndexError):
- node_calculations(points)
-
- def test_spline_curvature_calculations(self):
- vect = mockNodesData01.v
- dist_prev = mockNodesData01.dp
-
- curv, curv_ds = spline_curvature_calculations(vect, dist_prev)
-
- assert_array_almost_equal(curv, mockNodesData01.curv)
- assert_array_almost_equal(curv_ds, mockNodesData01.curv_ds)
-
- def test_spline_curvature_calculations_with_route_data(self):
- mockRouteData_02_01.reset()
- nodes_data = mockRouteData_02_01._nodes_data
- vect = np.column_stack((nodes_data[:, 4], nodes_data[:, 5]))
- dist_prev = nodes_data[:, 6]
-
- curv, curv_ds = spline_curvature_calculations(vect, dist_prev)
-
- assert_array_almost_equal(curv, mockRouteData_02_01._curv)
- assert_array_almost_equal(curv_ds, mockRouteData_02_01._curv_ds)
-
- def test_split_speed_section_by_sign(self):
- curv_sec = mockCurveSectionSin.curv_sec
- new_secs = split_speed_section_by_sign(curv_sec)
-
- # 3 sections with matching initial and final distance
- self.assertEqual(len(new_secs), 3)
- self.assertEqual(new_secs[0][0][2], mockCurveSectionSin.di)
- self.assertEqual(new_secs[2][-1][2], mockCurveSectionSin.df)
-
- # All new sections has same sign internally
- for sec in new_secs:
- self.assertEqual(np.average(sec, axis=0)[1], sec[0][1])
-
- # Sections change sign
- for idx in range(2):
- self.assertNotEqual(new_secs[idx][0][1], new_secs[idx + 1][0][1])
-
- # total items consistency
- lenghts = [len(sec) for sec in new_secs]
- self.assertEqual(len(curv_sec), sum(lenghts))
-
- def test_split_speed_section_by_curv_degree(self):
- curv_sec = mockCurveSteepCurvChange.curv_sec
- new_secs = split_speed_section_by_curv_degree(curv_sec)
-
- # 3 sections with matching initial and final distance
- self.assertEqual(len(new_secs), 3)
- self.assertEqual(new_secs[0][0][2], mockCurveSteepCurvChange.di)
- self.assertEqual(new_secs[2][-1][2], mockCurveSteepCurvChange.df)
-
- # Sections split at the right points
- split_dist = [sec[-1][2] for sec in new_secs]
- self.assertListEqual(split_dist, [50., 150., 200.])
-
- def test_split_speed_section_by_curv_degree_does_nothing_if_short(self):
- curv_sec = mockCurveSteepCurvChangeShort.curv_sec
- new_secs = split_speed_section_by_curv_degree(curv_sec)
-
- self.assertEqual(len(new_secs), 1)
- assert_array_almost_equal(curv_sec, new_secs[0])
-
- def test_split_speed_section_by_curv_degree_does_nothing_if_no_substantial_change(self):
- curv_sec = mockCurveSmoothCurveChange.curv_sec
- new_secs = split_speed_section_by_curv_degree(curv_sec)
-
- self.assertEqual(len(new_secs), 1)
- assert_array_almost_equal(curv_sec, new_secs[0])
-
- def test_speed_section(self):
- curv_sec = mockCurveSectionSin.curv_sec
-
- speed_secs = speed_section(curv_sec)
- expected = np.array([0., 1000., 1.51657509, 1.])
-
- assert_array_almost_equal(speed_secs, expected)
-
- def test_speed_limits_for_curvatures_data(self):
- curv = mockCurveSectionSin.curv
- curv_ds = mockCurveSectionSin.curv_ds
-
- expected = np.array([
- [10., 490., 1.51657509, 1.],
- [510., 990., 1.51657509, -1.]])
- limits = speed_limits_for_curvatures_data(curv, curv_ds)
-
- assert_array_almost_equal(limits, expected)
-
- def test_is_wr_a_valid_divertion_from_node(self):
- wr = WayRelation(mockOSMWay_02_01_CurvyTownWithIntersections)
- mockOSMWay_02_02_Divertion_34785115.tags['oneway'] = 'yes'
- wr_div = WayRelation(mockOSMWay_02_02_Divertion_34785115)
-
- # False if id already in route
- wr_ids = [wr.id, wr_div.id]
- self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 34785115, wr_ids))
-
- # True if id not in route, node_id is edge and not prohibited
- wr_ids = [wr.id, 11111, 22222]
- self.assertTrue(is_wr_a_valid_divertion_from_node(wr_div, 34785115, wr_ids))
-
- # False if id not in route, node_id is edge but prohibited (wrong direction from node 319503453)
- self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 319503453, wr_ids))
-
- # False if id not in route, node_id is not edge
- self.assertFalse(is_wr_a_valid_divertion_from_node(wr_div, 44444, wr_ids))
-
-
-class TestSpeedLimitSection(unittest.TestCase):
- def test_speed_limit_section_init(self):
- section = SpeedLimitSection(10., 20., 50.)
-
- self.assertEqual(section.start, 10.)
- self.assertEqual(section.end, 20.)
- self.assertEqual(section.value, 50.)
-
-
-class TestTurnSpeedLimitSection(unittest.TestCase):
- def test_turn_speed_limit_section_init(self):
- section = TurnSpeedLimitSection(10., 20., 50., -1.)
-
- self.assertEqual(section.start, 10.)
- self.assertEqual(section.end, 20.)
- self.assertEqual(section.value, 50.)
- self.assertEqual(section.curv_sign, -1.)
-
-
-class TestNodesData(unittest.TestCase):
- def test_init_with_empty_list(self):
- nd = NodesData([], {})
-
- self.assertEqual(len(nd._nodes_data), 0)
- num_diverstions = sum([len(d) for d in nd._divertions])
- self.assertEqual(num_diverstions, 0)
- self.assertEqual(len(nd._curvature_speed_sections_data), 0)
-
- def test_init_with_single_wr_includes_all_wr_nodes(self):
- mockRouteData_02_02_single_wr.reset()
- way_relations = mockRouteData_02_02_single_wr.wrs
- wr_index = mockRouteData_02_02_single_wr.way_collection.wr_index
-
- nd = NodesData(way_relations, wr_index)
-
- assert_array_almost_equal(nd._nodes_data, mockRouteData_02_02_single_wr._nodes_data)
- assert_array_almost_equal(nd._curvature_speed_sections_data,
- mockRouteData_02_02_single_wr._curvature_speed_sections_data)
- self.assertListEqual(nd._divertions, mockRouteData_02_02_single_wr._divertions)
- self.assertEqual(len(nd._nodes_data), len(way_relations[0].way.nodes))
- self.assertEqual(len(nd._curvature_speed_sections_data), 6)
- num_diverstions = sum([len(d) for d in nd._divertions])
- self.assertEqual(num_diverstions, 6)
-
- def test_init_with_less_than_4_nodes(self):
- wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way)
-
- nd = NodesData([wr_t], {})
-
- self.assertEqual(len(nd._nodes_data), 0)
- num_diverstions = sum([len(d) for d in nd._divertions])
- self.assertEqual(num_diverstions, 0)
- self.assertEqual(len(nd._curvature_speed_sections_data), 0)
-
- def test_init_with_multiple_wr(self):
- mockRouteData_02_01.reset()
- way_relations = mockRouteData_02_01.wrs
- wr_index = mockRouteData_02_01.way_collection.wr_index
-
- nd = NodesData(way_relations, wr_index)
-
- assert_array_almost_equal(nd._nodes_data, mockRouteData_02_01._nodes_data)
- assert_array_almost_equal(nd._curvature_speed_sections_data, mockRouteData_02_01._curvature_speed_sections_data)
- self.assertListEqual(nd._divertions, mockRouteData_02_01._divertions)
- self.assertEqual(len(nd._curvature_speed_sections_data), 9)
- num_diverstions = sum([len(d) for d in nd._divertions])
- self.assertEqual(num_diverstions, 14)
-
- def test_count(self):
- mockRouteData_02_01.reset()
- way_relations = mockRouteData_02_01.wrs
- wr_index = mockRouteData_02_01.way_collection.wr_index
- num_n = sum([len(wr.way.nodes) for wr in way_relations]) - len(way_relations) + 1
-
- nd = NodesData(way_relations, wr_index)
-
- self.assertEqual(nd.count, num_n)
-
- def test_get_on_empty(self):
- wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way)
-
- nd = NodesData([wr_t], {})
- assert_array_almost_equal(nd.get(NodeDataIdx.node_id), np.array([]))
-
- def test_get_values(self):
- mockRouteData_02_01.reset()
- way_relations = mockRouteData_02_01.wrs
- wr_index = mockRouteData_02_01.way_collection.wr_index
-
- nd = NodesData(way_relations, wr_index)
-
- assert_array_almost_equal(nd.get(NodeDataIdx.node_id), mockRouteData_02_01._nodes_data[:, 0])
- assert_array_almost_equal(nd.get(NodeDataIdx.lat), mockRouteData_02_01._nodes_data[:, 1])
- assert_array_almost_equal(nd.get(NodeDataIdx.lon), mockRouteData_02_01._nodes_data[:, 2])
- assert_array_almost_equal(nd.get(NodeDataIdx.speed_limit), mockRouteData_02_01._nodes_data[:, 3])
- assert_array_almost_equal(nd.get(NodeDataIdx.x), mockRouteData_02_01._nodes_data[:, 4])
- assert_array_almost_equal(nd.get(NodeDataIdx.y), mockRouteData_02_01._nodes_data[:, 5])
- assert_array_almost_equal(nd.get(NodeDataIdx.dist_prev), mockRouteData_02_01._nodes_data[:, 6])
- assert_array_almost_equal(nd.get(NodeDataIdx.dist_next), mockRouteData_02_01._nodes_data[:, 7])
- assert_array_almost_equal(nd.get(NodeDataIdx.dist_route), mockRouteData_02_01._nodes_data[:, 8])
- assert_array_almost_equal(nd.get(NodeDataIdx.bearing), mockRouteData_02_01._nodes_data[:, 9])
-
- def test_speed_limits_ahead_from_empty(self):
- wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way)
-
- nd = NodesData([wr_t], {})
- self.assertEqual(len(nd.speed_limits_ahead(1, 10.)), 0)
-
- def test_speed_limits_ahead(self):
- mockRouteData_02_03.reset()
- way_relations = mockRouteData_02_03.wrs
- wr_index = mockRouteData_02_03.way_collection.wr_index
-
- nd = NodesData(way_relations, wr_index)
-
- # empty when ahead_idx is none.
- self.assertEqual(len(nd.speed_limits_ahead(None, 10.)), 0)
-
- # All limist from 0
- all_limits = nd.speed_limits_ahead(1, nd.get(NodeDataIdx.dist_next)[0])
- self.assertEqual(len(all_limits), 4) # 4 limits on this mock road.
- self.assertListEqual([sl.value for sl in all_limits], [v * CV.KPH_TO_MS for v in [50, 100, 50, 100]])
- for idx, sl in enumerate(all_limits):
- self.assertTrue(sl.end > sl.start)
- self.assertTrue(sl.value > 0.)
- if idx == 0:
- self.assertEqual(sl.start, 0.)
- else:
- self.assertEqual(sl.start, all_limits[idx - 1].end)
- self.assertNotEqual(sl.value, all_limits[idx - 1].value)
-
- def test_distance_to_end_from_empty(self):
- wr_t = WayRelation(mockOSMWay_02_03_Short_3_node_way)
-
- nd = NodesData([wr_t], {})
- self.assertIsNone(nd.distance_to_end(1, 10.))
-
- def test_distance_to_end(self):
- mockRouteData_02_03.reset()
- way_relations = mockRouteData_02_03.wrs
- wr_index = mockRouteData_02_03.way_collection.wr_index
-
- nd = NodesData(way_relations, wr_index)
-
- # none when ahead_idx is none.
- self.assertIsNone(nd.distance_to_end(None, 10.))
-
- # From the begining
- expected = np.sum(nd.get(NodeDataIdx.dist_next))
- self.assertAlmostEqual(nd.distance_to_end(1, nd.get(NodeDataIdx.dist_next)[0]), expected)
- self.assertAlmostEqual(nd.get(NodeDataIdx.dist_route)[-1], expected)
-
- # From the node next to last
- expected = nd.get(NodeDataIdx.dist_next)[-2]
- self.assertAlmostEqual(nd.distance_to_end(nd.count - 2, 0.), expected)
-
- def test_distance_to_node(self):
- mockRouteData_02_03.reset()
- way_relations = mockRouteData_02_03.wrs
- wr_index = mockRouteData_02_03.way_collection.wr_index
-
- nd = NodesData(way_relations, wr_index)
- dist_to_node_ahead = 10.
- node_id = 1887995486 # Some node id in the middle of the way. idx 50
- node_idx = np.nonzero(nd.get(NodeDataIdx.node_id) == node_id)[0][0]
-
- # none when ahead_idx is none.
- self.assertIsNone(nd.distance_to_node(node_id, None, dist_to_node_ahead))
-
- # From the begining
- expected = nd.get(NodeDataIdx.dist_route)[node_idx]
- self.assertAlmostEqual(nd.distance_to_node(node_id, 1, nd.get(NodeDataIdx.dist_next)[0]), expected)
-
- # From the end
- expected = -np.sum(nd.get(NodeDataIdx.dist_next)[node_idx:])
- self.assertAlmostEqual(nd.distance_to_node(node_id, len(nd.get(NodeDataIdx.node_id)) - 1, 0.), expected)
-
- # From some node behind including dist to node ahead
- ahead_idx = node_idx - 10
- expected = np.sum(nd.get(NodeDataIdx.dist_next)[ahead_idx:node_idx]) + dist_to_node_ahead
- self.assertAlmostEqual(nd.distance_to_node(node_id, ahead_idx, dist_to_node_ahead), expected)
-
- # From some node ahead including dist to node ahead
- ahead_idx = node_idx + 10
- expected = -np.sum(nd.get(NodeDataIdx.dist_next)[node_idx:ahead_idx]) + dist_to_node_ahead
- self.assertAlmostEqual(nd.distance_to_node(node_id, ahead_idx, dist_to_node_ahead), expected)
-
-# TODO: Missing tests for curvatures_speed_limit_sections_ahead and possible_divertions
diff --git a/selfdrive/mapd/test/test_WayRelation.py b/selfdrive/mapd/test/test_WayRelation.py
deleted file mode 100644
index 9719c86f0..000000000
--- a/selfdrive/mapd/test/test_WayRelation.py
+++ /dev/null
@@ -1,651 +0,0 @@
-import copy
-import unittest
-import numpy as np
-from unittest import mock
-from numpy.testing import assert_array_almost_equal
-from datetime import datetime as dt, timezone, timedelta
-from common.conversions import Conversions as CV
-from selfdrive.mapd.lib.WayRelation import WayRelation, is_osm_time_condition_active, \
- conditional_speed_limit_for_osm_tag_limit_string, speed_limit_for_osm_tag_limit_string
-from selfdrive.mapd.config import LANE_WIDTH
-from selfdrive.mapd.lib.geo import DIRECTION, R, vectors
-from selfdrive.mapd.test.mock_data import mockOSMWay_01_01_LongCurvy, mockOSMWay_01_02_Loop, \
- mockOSMWay_02_01_CurvyTownWithIntersections
-
-
-class TestWayRelationFileFunctions(unittest.TestCase):
- def test_speed_limit_for_osm_tag_limit_string(self):
- values = [
- None, # Invalid
- "1000", # Invalid
- "60 kph", # Invalid
- "100",
- "30 mph",
- "DE:zone:40",
- "DE:zone:50 mph",
- "AR:urban",
- "CZ:pedestrian_zone",
- "DK:urban",
- "DK:rural",
- "DK:motorway",
- "DE:living_street",
- "DE:residential",
- "DE:urban",
- "DE:rural",
- "DE:trunk", # No limit
- "DE:motorway", # No limit
- "GB:nsl_restricted",
- "GB:nsl_single",
- "GB:nsl_dual",
- "GB:motorway",
- "GB:invalid", # Invalid
- ]
-
- expected = [
- 0.,
- 0.,
- 0.,
- 100. * CV.KPH_TO_MS,
- 30. * CV.MPH_TO_MS,
- 40. * CV.KPH_TO_MS,
- 50. * CV.MPH_TO_MS,
- 40. * CV.KPH_TO_MS,
- 20. * CV.KPH_TO_MS,
- 50. * CV.KPH_TO_MS,
- 80. * CV.KPH_TO_MS,
- 130. * CV.KPH_TO_MS,
- 7. * CV.KPH_TO_MS,
- 30. * CV.KPH_TO_MS,
- 50. * CV.KPH_TO_MS,
- 100. * CV.KPH_TO_MS,
- 0.,
- 0.,
- 30. * CV.MPH_TO_MS,
- 60. * CV.MPH_TO_MS,
- 70. * CV.MPH_TO_MS,
- 70. * CV.MPH_TO_MS,
- 0.,
- ]
-
- result = [speed_limit_for_osm_tag_limit_string(sls) for sls in values]
-
- self.assertEqual(result, expected)
-
- @mock.patch('selfdrive.mapd.lib.WayRelation.dt')
- def test_is_osm_time_condition_active(self, mock_dt):
- tz = timezone(timedelta(hours=1), 'berlin')
- wed_10_10_am = dt(2021, 9, 1, 10, 10, 0)
- mock_dt.now.return_value = wed_10_10_am
- mock_dt.tzinfo = tz
- mock_dt.combine = dt.combine
- mock_dt.strptime = dt.strptime
-
- values = [
- "WE", # Invalid
- "We",
- "Mo",
- "Fr",
- "Tu-Th",
- "10:00", # Invalid
- "10:00-10:30",
- "We 10:00-10:30",
- "SU 10:00-10:30", # Valid, SU string not considered a day string.
- "Sa 10:00-10:30",
- "Tu-Th 10:00-10:30",
- ]
-
- expected = [
- False, # Invalid
- True,
- False,
- False,
- True,
- False, # Invalid
- True,
- True,
- True,
- False,
- True,
- ]
-
- result = [is_osm_time_condition_active(cs) for cs in values]
-
- self.assertEqual(result, expected)
-
- @mock.patch('selfdrive.mapd.lib.WayRelation.dt')
- def test_conditional_speed_limit_for_osm_tag_limit_string(self, mock_dt):
- tz = timezone(timedelta(hours=1), 'berlin')
- wed_10_10_am = dt(2021, 9, 1, 10, 10, 0)
- mock_dt.now.return_value = wed_10_10_am
- mock_dt.tzinfo = tz
- mock_dt.combine = dt.combine
- mock_dt.strptime = dt.strptime
-
- values = [
- None, # Invalid
- "Hola", # Invalid
- "100 @ (WE)", # Invalid
- "x @ (We)", # Invalid
- "100 @ (We)",
- "100 @ (Mo)",
- "100 @ (Fr)",
- "100 @ (Tu-Th)",
- "100 @ (10:00)", # Invalid
- "100 @ (10:00-10:30)",
- "100 @ (We 10:00-10:30)",
- "100 @ (SU 10:00-10:30)", # Valid, SU string not considered a day string.
- "100 @ (Sa 10:00-10:30)",
- "100 @ (Tu-Th 10:00-10:30)",
- "100 @ (Mo-Th;Su)",
- "100 @ (Mo Th;Fr-Sa)",
- "100 @ (Fr-Su;Mo-Tu)",
- "100 @ (10:00-10:30;15:00-16:00)",
- "100 @ (We;Mo-Tu)",
- "100 @ (We 10:00-10:30;Th 15:00-16:00)",
- "100 @ (Tu 10:00-10:30;Th 15:00-16:00)",
- ]
-
- _100 = 100. * CV.KPH_TO_MS
-
- expected = [
- 0., # Invalid
- 0., # Invalid
- 0., # Invalid
- 0., # Invalid
- _100,
- 0.,
- 0.,
- _100,
- 0., # Invalid
- _100,
- _100,
- _100,
- 0.,
- _100,
- _100,
- _100,
- 0.,
- _100,
- _100,
- _100,
- 0.
- ]
-
- result = [conditional_speed_limit_for_osm_tag_limit_string(ls) for ls in values]
-
- self.assertEqual(result, expected)
-
-
-class TestWayRelation(unittest.TestCase):
- def test_way_relation_init(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
-
- nodes_np_expected = np.radians(np.array([[nd.lat, nd.lon] for nd in wayRelation.way.nodes], dtype=float))
- v = vectors(wayRelation._nodes_np)
- way_distances_expected = np.linalg.norm(v * R, axis=1)
- way_bearings_expected = np.arctan2(v[:, 0], v[:, 1])
- bbox_expected = np.array([
- [0.91321784, 0.2346417],
- [0.91344672, 0.23475751]])
-
- self.assertEqual(wayRelation.way.id, 179532213)
- self.assertIsNone(wayRelation.parent_wr_id)
- self.assertEqual(wayRelation.direction, DIRECTION.NONE)
- self.assertEqual(wayRelation._speed_limit, None)
- self.assertEqual(wayRelation._one_way, 'yes')
- self.assertEqual(wayRelation.name, None)
- self.assertEqual(wayRelation.ref, 'B 96')
- self.assertEqual(wayRelation.highway_type, 'trunk')
- self.assertEqual(wayRelation.highway_rank, 10)
- self.assertEqual(wayRelation.lanes, 2)
- assert_array_almost_equal(wayRelation._nodes_np, nodes_np_expected)
- assert_array_almost_equal(wayRelation._way_distances, way_distances_expected)
- assert_array_almost_equal(wayRelation._way_bearings, way_bearings_expected)
- assert_array_almost_equal(wayRelation.bbox, bbox_expected)
- self.assertEqual(wayRelation.edge_nodes_ids, [wayRelation.way.nodes[0].id, wayRelation.way.nodes[-1].id])
-
- def test_way_relation_init_with_parent(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy, parent=WayRelation(mockOSMWay_01_02_Loop))
-
- self.assertEqual(wayRelation.way.id, 179532213)
- self.assertEqual(wayRelation.parent_wr_id, 29233907)
-
- def test_way_relation_equality(self):
- wayRelation1 = WayRelation(mockOSMWay_01_01_LongCurvy)
- wayRelation2 = copy.copy(wayRelation1)
- wayRelation3 = copy.deepcopy(wayRelation1)
- wayRelation3.way.id = 123
-
- self.assertEqual(wayRelation1, wayRelation2)
- self.assertNotEqual(wayRelation1, wayRelation3)
-
- def test_way_relation_reset_location_variables(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- self.make_wayRelation_location_dirty(wayRelation)
-
- wayRelation.reset_location_variables()
-
- self.assert_wayRelation_variables_reset(wayRelation)
-
- def test_way_relation_id(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
-
- self.assertEqual(wayRelation.id, 179532213)
-
- def test_way_relation_road_name(self):
- # road name when no tag for name or ref
- wayRelation = WayRelation(mockOSMWay_01_02_Loop)
- self.assertIsNone(wayRelation.road_name)
- # road name based on ref tag
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- self.assertEqual(wayRelation.road_name, "B 96")
- # road name based on name tag
- wayRelation = WayRelation(mockOSMWay_02_01_CurvyTownWithIntersections)
- self.assertEqual(wayRelation.road_name, "Hauptstraße")
-
- def test_way_relation_update_resets_on_update(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- self.make_wayRelation_location_dirty(wayRelation)
- location_rad = np.array([0., 0.]) # Location outside bbox
-
- wayRelation.update(location_rad, 0., 10.)
-
- self.assertFalse(wayRelation.is_location_in_bbox(location_rad))
- self.assert_wayRelation_variables_reset(wayRelation)
-
- def test_way_relation_update_only_resets_if_no_possible_found(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- location_rad = wayRelation.bbox[0] # Location inside bbox but outside actual way (due to padding)
-
- wayRelation.update(location_rad, 0., 10.)
-
- self.assertTrue(wayRelation.is_location_in_bbox(location_rad))
- self.assert_wayRelation_variables_reset(wayRelation)
-
- def test_way_relation_updates_in_the_correct_direction_with_correct_property_values(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- location_rad = np.radians(np.array([52.32855593146639, 13.445320150125069]))
- bearing_rad = 0.
-
- wayRelation.update(location_rad, bearing_rad, 10.)
-
- self.assertTrue(wayRelation.is_location_in_bbox(location_rad))
- self.assertEqual(wayRelation.direction, DIRECTION.FORWARD)
- self.assertEqual(wayRelation.ahead_idx, 17)
- self.assertEqual(wayRelation.behind_idx, 16)
- self.assertAlmostEqual(wayRelation._distance_to_way, 3.43290781621360)
- self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.320717420388962)
- self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 25.4998961709014)
- self.assertTrue(wayRelation.active)
- self.assertFalse(wayRelation.diverting)
- assert_array_almost_equal(wayRelation.location_rad, location_rad)
- self.assertEqual(wayRelation.bearing_rad, bearing_rad)
- self.assertIsNone(wayRelation._speed_limit)
-
- bearing_rad = 180.
-
- wayRelation.update(location_rad, bearing_rad, 10.)
-
- self.assertTrue(wayRelation.is_location_in_bbox(location_rad))
- self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD)
- self.assertEqual(wayRelation.ahead_idx, 16)
- self.assertEqual(wayRelation.behind_idx, 17)
- self.assertAlmostEqual(wayRelation._distance_to_way, 3.43290781621360)
- self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.9507682562504284)
- self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 11.11623371145368)
- self.assertTrue(wayRelation.active)
- self.assertFalse(wayRelation.diverting)
- assert_array_almost_equal(wayRelation.location_rad, location_rad)
- self.assertEqual(wayRelation.bearing_rad, bearing_rad)
- self.assertIsNone(wayRelation._speed_limit)
-
- def test_way_relation_updates_with_location_closest_to_way_when_multiple_possible(self):
- wayRelation = WayRelation(mockOSMWay_01_02_Loop)
- location_rad = np.radians(np.array([52.313303275461564, 13.437729236325788]))
- bearing_rad = np.radians(10.)
-
- wayRelation.update(location_rad, bearing_rad, 10.)
-
- self.assertTrue(wayRelation.is_location_in_bbox(location_rad))
- self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD)
- self.assertEqual(wayRelation.ahead_idx, 26)
- self.assertEqual(wayRelation.behind_idx, 27)
- self.assertAlmostEqual(wayRelation._distance_to_way, 10.151775235257011)
- self.assertAlmostEqual(wayRelation._active_bearing_delta, 0.06371131069242782)
- self.assertAlmostEqual(wayRelation.distance_to_node_ahead, 10.174073707120915)
- self.assertTrue(wayRelation.active)
- self.assertFalse(wayRelation.diverting)
- assert_array_almost_equal(wayRelation.location_rad, location_rad)
- self.assertEqual(wayRelation.bearing_rad, bearing_rad)
- self.assertIsNone(wayRelation._speed_limit)
-
- def test_way_relation_updates_will_become_inactive_if_too_far_from_way(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- # Location is 24.9 mts away from the way. There are 2 Lanes in this way.
- location_rad = np.radians(np.array([52.328634560607746, 13.445609877522788]))
- location_stdev = 5.5 # threshold is 4 * location_stdev + LANE_WIDTH
- distance_threshold = 4. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2.
-
- wayRelation.update(location_rad, 0., location_stdev)
- self.assertTrue(wayRelation.active)
- self.assertLess(wayRelation._distance_to_way, distance_threshold)
-
- location_stdev = 5.
-
- wayRelation.update(location_rad, 0., location_stdev)
- self.assertFalse(wayRelation.active)
-
- def test_way_relation_updates_will_update_diverting_correctly(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- # Location is 24.9 mts away from the way. There are 2 Lanes in this way.
- location_rad = np.radians(np.array([52.328634560607746, 13.445609877522788]))
- location_stdev = 11.
- distance_threshold = 2. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2.
-
- wayRelation.update(location_rad, 0., location_stdev)
-
- self.assertLess(wayRelation._distance_to_way, distance_threshold)
- self.assertFalse(wayRelation.diverting)
-
- location_stdev = 10.
- distance_threshold = 2. * location_stdev + wayRelation.lanes * LANE_WIDTH / 2.
-
- wayRelation.update(location_rad, 0., location_stdev)
-
- self.assertGreater(wayRelation._distance_to_way, distance_threshold)
- self.assertTrue(wayRelation.diverting)
-
- def test_way_relation_update_direction_from_starting_node_resets_speed_limit(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- wayRelation._speed_limit = 10.
-
- wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[0].id)
-
- self.assertIsNone(wayRelation._speed_limit)
-
- def test_way_relation_update_direction_from_starting_node_updates_correctly(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[0].id)
- self.assertEqual(wayRelation.direction, DIRECTION.FORWARD)
-
- wayRelation.update_direction_from_starting_node(wayRelation.way.nodes[-1].id)
- self.assertEqual(wayRelation.direction, DIRECTION.BACKWARD)
-
- wayRelation.update_direction_from_starting_node(0)
- self.assertEqual(wayRelation.direction, DIRECTION.NONE)
-
- def test_way_relation_is_location_in_bbox(self):
- wayRelation = WayRelation(mockOSMWay_01_02_Loop)
- bbox = wayRelation.bbox
-
- loc_avg = np.average(bbox, axis=0)
- loc_min = np.min(bbox, axis=0)
- loc_max = np.max(bbox, axis=0)
-
- locations = [
- loc_avg,
- loc_min,
- loc_max,
- [loc_avg[0], loc_min[1]],
- [loc_avg[0], loc_max[1]],
- [loc_min[0], loc_avg[1]],
- [loc_max[0], loc_avg[1]],
- loc_min - 0.1,
- loc_max + 0.1,
- [loc_avg[0], loc_min[1] - 0.1],
- [loc_avg[0], loc_max[1] + 0.1],
- [loc_min[0] - 0.1, loc_avg[1]],
- [loc_max[0] + 0.1, loc_avg[1]],
- ]
-
- is_in = [wayRelation.is_location_in_bbox(loc) for loc in locations]
-
- self.assertEqual(is_in, [True, True, True, True, True, True, True, False, False, False, False, False, False])
-
- def test_way_relation_speed_limit_when_set(self):
- wayRelation = WayRelation(mockOSMWay_01_02_Loop)
- wayRelation._speed_limit = 10.
-
- self.assertEqual(wayRelation.speed_limit, 10.)
-
- @mock.patch('selfdrive.mapd.lib.WayRelation.dt')
- def test_way_relation_speed_limit_conditional(self, mock_dt):
- tz = timezone(timedelta(hours=1), 'berlin')
- wed_10_10_am = dt(2021, 9, 1, 10, 10, 0)
- mock_dt.now.return_value = wed_10_10_am
- mock_dt.tzinfo = tz
- mock_dt.combine = dt.combine
- mock_dt.strptime = dt.strptime
-
- # Reset all tags before teting
- mockOSMWay_01_02_Loop.tags = {}
- wayRelation = WayRelation(mockOSMWay_01_02_Loop)
-
- # No Value
- self.assertEqual(wayRelation.speed_limit, 0.)
-
- # Value on both directions
- wayRelation._speed_limit = None
- wayRelation.way.tags["maxspeed:conditional"] = "100 @ (We 10:00-10:30)"
- self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
-
- # Value on forward
- wayRelation.way.tags.pop("maxspeed:conditional")
- wayRelation._speed_limit = None
- wayRelation.direction = DIRECTION.FORWARD
- self.assertEqual(wayRelation.speed_limit, 0.)
-
- wayRelation._speed_limit = None
- wayRelation.way.tags["maxspeed:forward:conditional"] = "100 @ (We 10:00-10:30)"
- self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
-
- # Value on backward
- wayRelation._speed_limit = None
- wayRelation.direction = DIRECTION.BACKWARD
- self.assertEqual(wayRelation.speed_limit, 0.)
-
- wayRelation._speed_limit = None
- wayRelation.way.tags["maxspeed:backward:conditional"] = "100 @ (We 10:00-10:30)"
- self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
-
- def test_way_relation_speed_limit_maxspeed(self):
- # Reset all tags before teting
- mockOSMWay_01_02_Loop.tags = {}
- wayRelation = WayRelation(mockOSMWay_01_02_Loop)
-
- # No Value
- self.assertEqual(wayRelation.speed_limit, 0.)
-
- # Value on both directions
- wayRelation._speed_limit = None
- wayRelation.way.tags["maxspeed"] = "100"
- self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
-
- # Value on forward
- wayRelation.way.tags.pop("maxspeed")
- wayRelation._speed_limit = None
- wayRelation.direction = DIRECTION.FORWARD
- self.assertEqual(wayRelation.speed_limit, 0.)
-
- wayRelation._speed_limit = None
- wayRelation.way.tags["maxspeed:forward"] = "100"
- self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
-
- # Value on backward
- wayRelation._speed_limit = None
- wayRelation.direction = DIRECTION.BACKWARD
- self.assertEqual(wayRelation.speed_limit, 0.)
-
- wayRelation._speed_limit = None
- wayRelation.way.tags["maxspeed:backward"] = "100"
- self.assertEqual(wayRelation.speed_limit, 100. * CV.KPH_TO_MS)
-
- def test_way_relation_active_bearing_delta_reflects_internal_value(self):
- wayRelation = WayRelation(mockOSMWay_01_02_Loop)
- wayRelation._active_bearing_delta = 10.
- self.assertEqual(wayRelation.active_bearing_delta, 10.)
-
- def test_way_relation_is_one_way(self):
- # Setup initial tags
- mockOSMWay_01_02_Loop.tags = {
- 'oneway': 'yes',
- 'highway': 'unclassified'
- }
- wayRelation = WayRelation(mockOSMWay_01_02_Loop)
-
- # oneway = yes
- self.assertTrue(wayRelation.is_one_way)
-
- # oneway non existing
- wayRelation._one_way = None
- self.assertFalse(wayRelation.is_one_way)
-
- # highway = motorway
- wayRelation.highway_type = 'motorway'
- self.assertTrue(wayRelation.is_one_way)
-
- def test_way_relation_is_prohibited(self):
- # Setup initial tags
- mockOSMWay_01_02_Loop.tags = {
- 'oneway': 'yes'
- }
- wayRelation = WayRelation(mockOSMWay_01_02_Loop)
-
- # Direction undefined
- wayRelation.direction = DIRECTION.NONE
- self.assertTrue(wayRelation.is_prohibited)
-
- # oneway = yes
- wayRelation.direction = DIRECTION.BACKWARD
- self.assertTrue(wayRelation.is_prohibited)
-
- wayRelation.direction = DIRECTION.FORWARD
- self.assertFalse(wayRelation.is_prohibited)
-
- # oneway non existing
- wayRelation._one_way = None
- self.assertFalse(wayRelation.is_one_way)
-
- wayRelation.direction = DIRECTION.BACKWARD
- self.assertFalse(wayRelation.is_prohibited)
-
- def test_way_relation_distance_to_way_reflects_internal_value(self):
- wayRelation = WayRelation(mockOSMWay_01_02_Loop)
- wayRelation._distance_to_way = 10.
- self.assertEqual(wayRelation.distance_to_way, 10.)
-
- def test_way_relation_node_ahead(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- # ahead_ids is None on init
- self.assertIsNone(wayRelation.node_ahead)
-
- wayRelation.ahead_idx = 15
- self.assertEqual(wayRelation.node_ahead, wayRelation.way.nodes[15])
-
- def test_way_relation_last_node(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- # direction is NONE on init
- self.assertIsNone(wayRelation.last_node)
-
- # forward
- wayRelation.direction = DIRECTION.FORWARD
- self.assertEqual(wayRelation.last_node, wayRelation.way.nodes[-1])
-
- # backward
- wayRelation.direction = DIRECTION.BACKWARD
- self.assertEqual(wayRelation.last_node, wayRelation.way.nodes[0])
-
- def test_way_relation_last_node_coordinates(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- # direction is NONE on init
- self.assertIsNone(wayRelation.last_node_coordinates)
-
- # forward
- wayRelation.direction = DIRECTION.FORWARD
- coords = np.radians(np.array([wayRelation.way.nodes[-1].lat, wayRelation.way.nodes[-1].lon], dtype=float))
- assert_array_almost_equal(wayRelation.last_node_coordinates, coords)
-
- # backward
- wayRelation.direction = DIRECTION.BACKWARD
- coords = np.radians(np.array([wayRelation.way.nodes[0].lat, wayRelation.way.nodes[0].lon], dtype=float))
- assert_array_almost_equal(wayRelation.last_node_coordinates, coords)
-
- def test_way_relation_node_before_edge_coordinates(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
-
- coords = wayRelation.node_before_edge_coordinates(0)
- assert_array_almost_equal(coords, np.array([0., 0.]))
-
- coords = wayRelation.node_before_edge_coordinates(wayRelation.way.nodes[0].id)
- coords_e = np.radians(np.array([wayRelation.way.nodes[1].lat, wayRelation.way.nodes[1].lon], dtype=float))
- assert_array_almost_equal(coords, coords_e)
-
- coords = wayRelation.node_before_edge_coordinates(wayRelation.way.nodes[-1].id)
- coords_e = np.radians(np.array([wayRelation.way.nodes[-2].lat, wayRelation.way.nodes[-2].lon], dtype=float))
- assert_array_almost_equal(coords, coords_e)
-
- def test_way_relation_split_no_matching_node(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
-
- wrs = wayRelation.split(0)
- self.assertEqual(len(wrs), 0)
-
- def test_way_relation_split_use_correct_ids(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
-
- wrs = wayRelation.split(wayRelation._nodes_ids[5], [-100, -200])
- self.assertEqual(wrs[0].id, -100)
- self.assertEqual(wrs[1].id, -200)
-
- def test_way_relation_split_on_edge_node(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- edge_node_ids = wayRelation.edge_nodes_ids
-
- for edge_node_id in edge_node_ids:
- wrs = wayRelation.split(edge_node_id)
- self.assertEqual(len(wrs), 1)
- self.assertEqual(wrs[0], wayRelation)
- self.assertEqual(wrs[0].way.tags, wayRelation.way.tags)
-
- def test_way_relation_split_on_internal_node(self):
- wayRelation = WayRelation(mockOSMWay_01_01_LongCurvy)
- way_ids = [-10, -20]
-
- for idx, node_id in enumerate(wayRelation._nodes_ids):
- if idx == 0 or idx == len(wayRelation._nodes_ids) - 1:
- continue
- wrs = wayRelation.split(node_id, way_ids)
- self.assertEqual(len(wrs), 2)
- assert_array_almost_equal(wrs[0]._nodes_ids, wayRelation._nodes_ids[:idx + 1])
- assert_array_almost_equal(wrs[1]._nodes_ids, wayRelation._nodes_ids[idx:])
- self.assertIn(node_id, wrs[0].edge_nodes_ids)
- self.assertIn(node_id, wrs[1].edge_nodes_ids)
- self.assertEqual(wrs[0].way.tags, wayRelation.way.tags)
- self.assertEqual(wrs[1].way.tags, wayRelation.way.tags)
- self.assertEqual(way_ids, [wr.id for wr in wrs])
-
- # Helpers
- def make_wayRelation_location_dirty(self, wayRelation):
- wayRelation.distance_to_node_ahead = 10.
- wayRelation.location_rad = 0.8
- wayRelation.bearing_rad = 2.
- wayRelation.active = True
- wayRelation.diverting = True
- wayRelation.ahead_idx = 5
- wayRelation.behind_idx = 4
- wayRelation._active_bearing_delta = 3.
- wayRelation._distance_to_way = 20.
-
- def assert_wayRelation_variables_reset(self, wayRelation):
- self.assertEqual(wayRelation.distance_to_node_ahead, 0.)
- self.assertIsNone(wayRelation.location_rad)
- self.assertIsNone(wayRelation.bearing_rad)
- self.assertFalse(wayRelation.active)
- self.assertFalse(wayRelation.diverting)
- self.assertIsNone(wayRelation.ahead_idx)
- self.assertIsNone(wayRelation.behind_idx)
- self.assertIsNone(wayRelation._active_bearing_delta)
- self.assertIsNone(wayRelation._distance_to_way)
-
- def wayRelation_mid_point_rad(self, wayRelation):
- return np.average(wayRelation.bbox, axis=0)
diff --git a/selfdrive/mapd/test/test_WayRelationIndex.py b/selfdrive/mapd/test/test_WayRelationIndex.py
deleted file mode 100644
index faa70c141..000000000
--- a/selfdrive/mapd/test/test_WayRelationIndex.py
+++ /dev/null
@@ -1,74 +0,0 @@
-import unittest
-from selfdrive.mapd.lib.WayRelationIndex import WayRelationIndex
-from selfdrive.mapd.test.mock_data import mockWayCollection01
-
-
-class TestWayRelationIndex(unittest.TestCase):
- def test_init_and_add(self):
- wrs = mockWayCollection01.way_relations
- wr_index = WayRelationIndex(wrs)
-
- # exptected init logic, including add logic.
- edge_nodes_index_dict = {}
- full_nodes_index_dict = {}
- for wr in wrs:
- for node in wr.way.nodes:
- node_id = node.id
- full_nodes_index_dict[node_id] = full_nodes_index_dict.get(node_id, []) + [wr]
- if node_id in wr.edge_nodes_ids:
- edge_nodes_index_dict[node_id] = edge_nodes_index_dict.get(node_id, []) + [wr]
-
- # assert logic delivers same result
- self.assertDictEqual(edge_nodes_index_dict, wr_index._edge_nodes_index_dict)
- self.assertDictEqual(full_nodes_index_dict, wr_index._full_nodes_index_dict)
- self.assertEqual(len(wr_index._edge_nodes_index_dict), 586)
- self.assertEqual(len(wr_index._full_nodes_index_dict), 2342)
-
- def test_remove(self):
- wrs = mockWayCollection01.way_relations
- wr_index = WayRelationIndex(wrs)
-
- wr_to_remove = wrs[0]
- affected_full_node_ids = [nd.id for nd in wr_to_remove.way.nodes]
- affected_edge_node_ids = wr_to_remove.edge_nodes_ids
-
- initial_full_lists = [wr_index._full_nodes_index_dict[ndid] for ndid in affected_full_node_ids]
- initial_edge_lists = [wr_index._edge_nodes_index_dict[ndid] for ndid in affected_edge_node_ids]
-
- expected_final_full_lists = [[wr for wr in li if wr is not wr_to_remove] for li in initial_full_lists]
- expected_final_edge_lists = [[wr for wr in li if wr is not wr_to_remove] for li in initial_edge_lists]
-
- wr_index.remove(wr_to_remove)
-
- final_full_lists = [wr_index._full_nodes_index_dict[ndid] for ndid in affected_full_node_ids]
- final_edge_lists = [wr_index._edge_nodes_index_dict[ndid] for ndid in affected_edge_node_ids]
-
- for idx, li in enumerate(final_full_lists):
- self.assertListEqual(li, expected_final_full_lists[idx])
-
- for idx, li in enumerate(final_edge_lists):
- self.assertListEqual(li, expected_final_edge_lists[idx])
-
- def test_way_relations_with_edge_node_id(self):
- wr_index = WayRelationIndex([])
- ref_dict = {
- 0: ["fake_wr1", "fake_wr2"],
- 1: ["fake_wr3"],
- 3: ["fake_wr4", "fake_wr5", "fake_wr6"],
- }
- wr_index._edge_nodes_index_dict = ref_dict
-
- for key, li in ref_dict.items():
- self.assertListEqual(li, wr_index.way_relations_with_edge_node_id(key))
-
- def test_way_relations_with_node_id(self):
- wr_index = WayRelationIndex([])
- ref_dict = {
- 0: ["fake_wr1", "fake_wr2"],
- 1: ["fake_wr3"],
- 3: ["fake_wr4", "fake_wr5", "fake_wr6"],
- }
- wr_index._full_nodes_index_dict = ref_dict
-
- for key, li in ref_dict.items():
- self.assertListEqual(li, wr_index.way_relations_with_node_id(key))
diff --git a/selfdrive/mapd/test/test_geo.py b/selfdrive/mapd/test/test_geo.py
deleted file mode 100644
index a18fe717b..000000000
--- a/selfdrive/mapd/test/test_geo.py
+++ /dev/null
@@ -1,234 +0,0 @@
-import unittest
-from selfdrive.mapd.lib.geo import vectors, ref_vectors, bearing_to_points, distance_to_points
-import numpy as np
-from numpy.testing import assert_array_almost_equal
-from selfdrive.mapd.test.mock_data import mockNodesData01
-
-
-class TestMapsdGeoLibrary(unittest.TestCase):
- def test_vectors(self):
- points = mockNodesData01.radians
- expected = np.array([
- [-1.34011951e-05, 1.00776468e-05],
- [-5.83610920e-06, 4.41046897e-06],
- [-7.83348567e-06, 5.94114032e-06],
- [-7.08560788e-06, 5.30408795e-06],
- [-6.57632550e-06, 4.05791838e-06],
- [-1.16077872e-06, 6.91151252e-07],
- [-1.53178098e-05, 9.62215139e-06],
- [-5.76314175e-06, 3.55176643e-06],
- [-1.61124141e-05, 9.86127759e-06],
- [-1.48006628e-05, 8.58192512e-06],
- [-1.72237209e-06, 1.60570482e-06],
- [-8.68985228e-06, 9.22062311e-06],
- [-1.42922812e-06, 1.51494711e-06],
- [-3.39761486e-06, 2.57087743e-06],
- [-2.75467373e-06, 1.28631255e-06],
- [-1.57501989e-05, 5.72309451e-06],
- [-2.52143954e-06, 1.34565295e-06],
- [-1.65278643e-06, 1.28630942e-06],
- [-2.22196114e-05, 1.64360838e-05],
- [-5.88675934e-06, 4.08234746e-06],
- [-1.83673390e-06, 1.46782408e-06],
- [-1.55004206e-06, 1.51843800e-06],
- [-1.20451533e-06, 2.06298011e-06],
- [-1.91801338e-06, 4.64083285e-06],
- [-2.38653483e-06, 5.60076524e-06],
- [-1.65269781e-06, 5.78402290e-06],
- [-3.66908309e-07, 2.75412965e-06],
- [0.00000000e+00, 1.92858882e-06],
- [9.09242615e-08, 2.66162711e-06],
- [3.14490354e-07, 1.53065382e-06],
- [8.66452477e-08, 4.83456208e-07],
- [2.41750593e-07, 1.10828411e-06],
- [7.43745228e-06, 1.27618831e-05],
- [5.59968054e-06, 9.63947367e-06],
- [2.01951467e-06, 2.75413219e-06],
- [4.59952643e-07, 6.42281301e-07],
- [1.74353749e-06, 1.74533121e-06],
- [2.57144338e-06, 2.11185266e-06],
- [1.46893187e-05, 1.11999169e-05],
- [3.84659229e-05, 2.85527952e-05],
- [2.71627936e-05, 1.98727946e-05],
- [8.44632540e-06, 6.15058628e-06],
- [2.29420323e-06, 1.92859222e-06],
- [2.58083439e-06, 3.16952222e-06],
- [3.76373643e-06, 5.14174911e-06],
- [5.32416098e-06, 6.51707770e-06],
- [8.62890928e-06, 1.11998258e-05],
- [1.25762497e-05, 1.65231340e-05],
- [8.90452991e-06, 1.10148240e-05],
- [4.86505726e-06, 4.59023120e-06],
- [3.85545276e-06, 3.39642031e-06],
- [3.48753893e-06, 3.30566145e-06],
- [2.99557303e-06, 2.61276368e-06],
- [2.15496788e-06, 1.87797727e-06],
- [4.10564937e-06, 3.58142649e-06],
- [1.53680853e-06, 1.33866906e-06],
- [4.99540175e-06, 4.35635790e-06],
- [1.37744970e-06, 1.19380643e-06],
- [1.74319821e-06, 1.28456429e-06],
- [9.99931238e-07, 1.14493663e-06],
- [6.42735560e-07, 1.19380547e-06],
- [3.66818436e-07, 1.46782199e-06],
- [5.45413874e-08, 1.83783170e-06],
- [-1.35818548e-07, 1.14842666e-06],
- [-5.50758101e-07, 3.02989178e-06],
- [-4.58785270e-07, 2.66162724e-06],
- [-2.51315555e-07, 1.19031459e-06],
- [-3.91409773e-07, 1.65457223e-06],
- [-2.14525206e-06, 5.67755902e-06],
- [-4.24558096e-07, 1.39102753e-06],
- [-1.46936730e-06, 5.32325561e-06],
- [-1.37632061e-06, 4.59021715e-06],
- [-8.26642899e-07, 4.68097349e-06],
- [-6.42702724e-07, 4.95673534e-06],
- [-3.66796960e-07, 7.25009780e-06],
- [-1.82861669e-07, 8.99542699e-06],
- [4.09564134e-07, 6.11214315e-06],
- [7.80629912e-08, 1.45734993e-06],
- [4.81205526e-07, 7.56076647e-06],
- [2.01036346e-07, 2.42775302e-06]])
-
- v = vectors(points)
- assert_array_almost_equal(v, expected)
-
- def test_ref_vectors(self):
- points = mockNodesData01.radians
- expected = np.array([
- [1.59924145e-04, -1.07153714e-04],
- [1.46520873e-04, -9.70788297e-05],
- [1.40683931e-04, -9.26694631e-05],
- [1.32849368e-04, -8.67297434e-05],
- [1.25762852e-04, -8.14268689e-05],
- [1.19185869e-04, -7.73700167e-05],
- [1.18024984e-04, -7.66790438e-05],
- [1.02705711e-04, -6.70592230e-05],
- [9.69420991e-05, -6.35082196e-05],
- [8.08284530e-05, -5.36489556e-05],
- [6.60268961e-05, -4.50685727e-05],
- [6.43043874e-05, -4.34630144e-05],
- [5.56137708e-05, -3.42431117e-05],
- [5.41844341e-05, -3.27282671e-05],
- [5.07866397e-05, -3.01576270e-05],
- [4.80318817e-05, -2.88714948e-05],
- [3.22813286e-05, -2.31493755e-05],
- [2.97598330e-05, -2.18038275e-05],
- [2.81069973e-05, -2.05175815e-05],
- [5.88679032e-06, -4.08230278e-06],
- [0.00000000e+00, 0.00000000e+00],
- [-1.83673390e-06, 1.46782408e-06],
- [-3.38677236e-06, 2.98626574e-06],
- [-4.59127869e-06, 5.04925111e-06],
- [-6.50926460e-06, 9.69009532e-06],
- [-8.89575243e-06, 1.52908806e-05],
- [-1.05483839e-05, 2.10749224e-05],
- [-1.09152548e-05, 2.38290571e-05],
- [-1.09152276e-05, 2.57576459e-05],
- [-1.08242659e-05, 2.84192717e-05],
- [-1.05097542e-05, 2.99499212e-05],
- [-1.04231024e-05, 3.04333762e-05],
- [-1.01813369e-05, 3.15416571e-05],
- [-2.74371711e-06, 4.43034426e-05],
- [2.85599752e-06, 5.39428964e-05],
- [4.87550206e-06, 5.66970360e-05],
- [5.33545066e-06, 5.73393202e-05],
- [7.07897615e-06, 5.90846634e-05],
- [9.65040026e-06, 6.11965396e-05],
- [2.43395796e-05, 7.23966392e-05],
- [6.28046063e-05, 1.00950641e-04],
- [8.99657904e-05, 1.20825635e-04],
- [9.84114021e-05, 1.26977201e-04],
- [1.00705361e-04, 1.28906084e-04],
- [1.03285783e-04, 1.32075942e-04],
- [1.07048835e-04, 1.37218192e-04],
- [1.12372096e-04, 1.43736004e-04],
- [1.20999382e-04, 1.54937080e-04],
- [1.33573053e-04, 1.71462176e-04],
- [1.42475686e-04, 1.82478533e-04],
- [1.47339899e-04, 1.87069658e-04],
- [1.51194707e-04, 1.90466811e-04],
- [1.54681601e-04, 1.93773152e-04],
- [1.57676653e-04, 1.96386513e-04],
- [1.59831239e-04, 1.98264929e-04],
- [1.63936150e-04, 2.01847201e-04],
- [1.65472675e-04, 2.03186195e-04],
- [1.70467147e-04, 2.07543619e-04],
- [1.71844334e-04, 2.08737728e-04],
- [1.73587247e-04, 2.10022678e-04],
- [1.74586922e-04, 2.11167839e-04],
- [1.75229389e-04, 2.12361789e-04],
- [1.75595876e-04, 2.13829694e-04],
- [1.75650001e-04, 2.15667538e-04],
- [1.75513922e-04, 2.16815933e-04],
- [1.74962478e-04, 2.19845700e-04],
- [1.74503092e-04, 2.22507224e-04],
- [1.74251509e-04, 2.23697482e-04],
- [1.73859727e-04, 2.25351966e-04],
- [1.71713202e-04, 2.31029044e-04],
- [1.71288336e-04, 2.32419977e-04],
- [1.69817793e-04, 2.37742908e-04],
- [1.68440467e-04, 2.42332824e-04],
- [1.67612807e-04, 2.47013617e-04],
- [1.66969033e-04, 2.51970213e-04],
- [1.66600674e-04, 2.59220232e-04],
- [1.66415880e-04, 2.68215619e-04],
- [1.66824132e-04, 2.74327850e-04],
- [1.66901881e-04, 2.75785216e-04],
- [1.67381459e-04, 2.83346086e-04],
- [1.67581971e-04, 2.85773882e-04]])
-
- v = ref_vectors(points[20], points)
- assert_array_almost_equal(v, expected)
-
- def test_bearing_to_points(self):
- points = mockNodesData01.radians
- expected = np.array([
- 2.16112265, 2.15595027, 2.15326799, 2.14916735, 2.14538642,
- 2.14657678, 2.14694997, 2.1492257, 2.1507589, 2.15676899,
- 2.16973441, 2.1651606, 2.12270237, 2.11416356, 2.10665211,
- 2.11201708, 2.19291574, 2.2031069, 2.20136186, 2.17712517,
- 0., -0.8965745, -0.84815954, -0.73792895, -0.59150953,
- -0.5269061, -0.46406215, -0.42954043, -0.4008254, -0.36391371,
- -0.33748609, -0.32996807, -0.31223189, -0.06185112, 0.05289544,
- 0.08578116, 0.0927833, 0.11924233, 0.15640718, 0.32432622,
- 0.55653415, 0.64003094, 0.6593301, 0.66319086, 0.66367982,
- 0.66251077, 0.66354137, 0.66302176, 0.66181884, 0.66291139,
- 0.66714676, 0.67095594, 0.67367984, 0.6765003, 0.67847961,
- 0.68212344, 0.68345356, 0.68762778, 0.68876073, 0.69070183,
- 0.69085143, 0.68988665, 0.68753177, 0.68348884, 0.68051081,
- 0.67220053, 0.66506824, 0.66177969, 0.65712162, 0.63916951,
- 0.6351146, 0.62025347, 0.60741567, 0.59618923, 0.58521935,
- 0.57122582, 0.55532475, 0.54636839, 0.54422542, 0.53357655,
- 0.53037033])
-
- v = bearing_to_points(points[20], points)
- assert_array_almost_equal(v, expected)
-
- def test_distance_to_points(self):
- points = mockNodesData01.radians
- expected = np.array([
- 1226.82569068, 1120.13820773, 1073.61121415, 1011.10016574,
- 954.81557436, 905.58045038, 896.97734399, 781.7102819,
- 738.58271117, 618.26145463, 509.47052142, 494.6403804,
- 416.22483123, 403.42108699, 376.42615499, 357.15106681,
- 253.15957483, 235.11572972, 221.77439728, 45.65465979,
- 0., 14.98414, 28.77606056, 43.49299446,
- 74.39463425, 112.74005248, 150.19482607, 167.03665191,
- 178.28443483, 193.80834084, 202.28154097, 205.01173833,
- 211.22777104, 282.88676739, 344.25957352, 362.66370657,
- 367.00206795, 379.23951996, 394.82505328, 486.76073331,
- 757.70254732, 960.03439155, 1023.81434529, 1042.49401713,
- 1068.53770096, 1109.12696535, 1162.74555108, 1252.847351,
- 1385.17179405, 1475.42502599, 1517.57849916, 1549.79838056,
- 1580.12405964, 1605.05483058, 1622.98937809, 1657.19268821,
- 1669.99157205, 1711.63883132, 1723.09133393, 1736.47655688,
- 1746.16073119, 1754.63481838, 1763.34186103, 1772.62691273,
- 1777.76189094, 1790.62024447, 1802.11488235, 1807.1040605,
- 1813.90756815, 1834.49265566, 1840.00708445, 1861.96087374,
- 1880.81678093, 1902.42091191, 1926.37194131, 1963.78301115,
- 2011.62679077, 2046.18028824, 2054.37811294, 2097.30347724,
- 2111.28586072])
-
- v = distance_to_points(points[20], points)
- assert_array_almost_equal(v, expected)
diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py
index 35eee5b03..836ed9cc4 100755
--- a/selfdrive/monitoring/dmonitoringd.py
+++ b/selfdrive/monitoring/dmonitoringd.py
@@ -3,10 +3,10 @@ import gc
import cereal.messaging as messaging
from cereal import car
+from cereal import log
from common.params import Params, put_bool_nonblocking
from common.realtime import set_realtime_priority
from selfdrive.controls.lib.events import Events
-from selfdrive.locationd.calibrationd import Calibration
from selfdrive.monitoring.driver_monitor import DriverStatus
@@ -22,7 +22,7 @@ def dmonitoringd_thread(sm=None, pm=None):
driver_status = DriverStatus(rhd_saved=Params().get_bool("IsRhdDetected"))
- sm['liveCalibration'].calStatus = Calibration.INVALID
+ sm['liveCalibration'].calStatus = log.LiveCalibrationData.Status.invalid
sm['liveCalibration'].rpyCalib = [0, 0, 0]
sm['carState'].buttonEvents = []
sm['carState'].standstill = True
diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py
index 53afaf9dd..5637b8115 100644
--- a/selfdrive/thermald/fan_controller.py
+++ b/selfdrive/thermald/fan_controller.py
@@ -11,7 +11,7 @@ from selfdrive.controls.lib.pid import PIDController
class BaseFanController(ABC):
@abstractmethod
- def update(self, max_cpu_temp: float, ignition: bool) -> int:
+ def update(self, cur_temp: float, ignition: bool) -> int:
pass
@@ -21,19 +21,19 @@ class TiciFanController(BaseFanController):
cloudlog.info("Setting up TICI fan handler")
self.last_ignition = False
- self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
+ self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_TRML))
- def update(self, max_cpu_temp: float, ignition: bool) -> int:
- self.controller.neg_limit = -(80 if ignition else 30)
+ def update(self, cur_temp: float, ignition: bool) -> int:
+ self.controller.neg_limit = -(100 if ignition else 30)
self.controller.pos_limit = -(30 if ignition else 0)
if ignition != self.last_ignition:
self.controller.reset()
- error = 70 - max_cpu_temp
+ error = 70 - cur_temp
fan_pwr_out = -int(self.controller.update(
error=error,
- feedforward=interp(max_cpu_temp, [60.0, 100.0], [0, -80])
+ feedforward=interp(cur_temp, [60.0, 100.0], [0, -100])
))
self.last_ignition = ignition
diff --git a/selfdrive/ui/_ui b/selfdrive/ui/_ui
index bfa2f4637..5c7b6583d 100755
Binary files a/selfdrive/ui/_ui and b/selfdrive/ui/_ui differ
diff --git a/selfdrive/ui/qt/spinner b/selfdrive/ui/qt/spinner
index 805088eb9..8e7061187 100755
Binary files a/selfdrive/ui/qt/spinner and b/selfdrive/ui/qt/spinner differ
diff --git a/selfdrive/ui/qt/text b/selfdrive/ui/qt/text
index 8e2333969..1cd75eb92 100755
Binary files a/selfdrive/ui/qt/text and b/selfdrive/ui/qt/text differ
diff --git a/selfdrive/ui/soundd/_soundd b/selfdrive/ui/soundd/_soundd
index bb13b220f..0f58ba01e 100755
Binary files a/selfdrive/ui/soundd/_soundd and b/selfdrive/ui/soundd/_soundd differ
diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts
index ae7b07ab1..46a2ed414 100644
--- a/selfdrive/ui/translations/main_de.ts
+++ b/selfdrive/ui/translations/main_de.ts
@@ -109,6 +109,13 @@
IP Adresse
+
+ CarSelectionPanel
+
+ [AUTO SELECT]
+
+
+
ConfirmationDialog
@@ -659,6 +666,14 @@ This may take up to a minute.
Navigation
Navigation
+
+ Vehicle Model:
+
+
+
+ [AUTO SELECT]
+
+
Setup
@@ -1079,6 +1094,33 @@ This may take up to a minute.
Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat.
+
+ Enable ALKA
+
+
+
+ When enabled, openpilot lateral Control will be always on when ACC MAIN is ON.
+Reboot required.
+
+
+
+ Enable MapD
+
+
+
+ When enabled, openpilot will display current road name and speed limit on the screen.
+Reboot required.
+
+
+
+ Enable Lane Priority Mode
+
+
+
+ When enabled, openpilot will use lane lines for lateral control, fallback to laneless mode automatically when lane lines probabilities are low.
+Reboot required.
+
+
Updater
diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts
index f82ea6d15..49723ce98 100644
--- a/selfdrive/ui/translations/main_ja.ts
+++ b/selfdrive/ui/translations/main_ja.ts
@@ -109,6 +109,13 @@
IP アドレス
+
+ CarSelectionPanel
+
+ [AUTO SELECT]
+
+
+
ConfirmationDialog
@@ -657,6 +664,14 @@ This may take up to a minute.
Navigation
ナビゲーション
+
+ Vehicle Model:
+
+
+
+ [AUTO SELECT]
+
+
Setup
@@ -1073,6 +1088,33 @@ This may take up to a minute.
Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat.
+
+ Enable ALKA
+
+
+
+ When enabled, openpilot lateral Control will be always on when ACC MAIN is ON.
+Reboot required.
+
+
+
+ Enable MapD
+
+
+
+ When enabled, openpilot will display current road name and speed limit on the screen.
+Reboot required.
+
+
+
+ Enable Lane Priority Mode
+
+
+
+ When enabled, openpilot will use lane lines for lateral control, fallback to laneless mode automatically when lane lines probabilities are low.
+Reboot required.
+
+
Updater
diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts
index b00c4165c..a126888cc 100644
--- a/selfdrive/ui/translations/main_ko.ts
+++ b/selfdrive/ui/translations/main_ko.ts
@@ -109,6 +109,13 @@
IP 주소
+
+ CarSelectionPanel
+
+ [AUTO SELECT]
+
+
+
ConfirmationDialog
@@ -658,6 +665,14 @@ This may take up to a minute.
Navigation
네비게이션
+
+ Vehicle Model:
+
+
+
+ [AUTO SELECT]
+
+
Setup
@@ -1074,6 +1089,33 @@ This may take up to a minute.
Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat.
+
+ Enable ALKA
+
+
+
+ When enabled, openpilot lateral Control will be always on when ACC MAIN is ON.
+Reboot required.
+
+
+
+ Enable MapD
+
+
+
+ When enabled, openpilot will display current road name and speed limit on the screen.
+Reboot required.
+
+
+
+ Enable Lane Priority Mode
+
+
+
+ When enabled, openpilot will use lane lines for lateral control, fallback to laneless mode automatically when lane lines probabilities are low.
+Reboot required.
+
+
Updater
diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts
index bb00b1878..7f7e92acf 100644
--- a/selfdrive/ui/translations/main_pt-BR.ts
+++ b/selfdrive/ui/translations/main_pt-BR.ts
@@ -109,6 +109,13 @@
Endereço IP
+
+ CarSelectionPanel
+
+ [AUTO SELECT]
+
+
+
ConfirmationDialog
@@ -662,6 +669,14 @@ Isso pode levar até um minuto.
Navigation
Navegação
+
+ Vehicle Model:
+
+
+
+ [AUTO SELECT]
+
+
Setup
@@ -1078,6 +1093,33 @@ Isso pode levar até um minuto.
Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat.
+
+ Enable ALKA
+
+
+
+ When enabled, openpilot lateral Control will be always on when ACC MAIN is ON.
+Reboot required.
+
+
+
+ Enable MapD
+
+
+
+ When enabled, openpilot will display current road name and speed limit on the screen.
+Reboot required.
+
+
+
+ Enable Lane Priority Mode
+
+
+
+ When enabled, openpilot will use lane lines for lateral control, fallback to laneless mode automatically when lane lines probabilities are low.
+Reboot required.
+
+
Updater
diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts
index 1a0aff8f8..06bf7a3bc 100644
--- a/selfdrive/ui/translations/main_zh-CHS.ts
+++ b/selfdrive/ui/translations/main_zh-CHS.ts
@@ -109,6 +109,13 @@
IP地址
+
+ CarSelectionPanel
+
+ [AUTO SELECT]
+
+
+
ConfirmationDialog
@@ -655,6 +662,14 @@ This may take up to a minute.
Navigation
导航
+
+ Vehicle Model:
+
+
+
+ [AUTO SELECT]
+
+
Setup
@@ -1071,6 +1086,33 @@ This may take up to a minute.
Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat.
+
+ Enable ALKA
+
+
+
+ When enabled, openpilot lateral Control will be always on when ACC MAIN is ON.
+Reboot required.
+
+
+
+ Enable MapD
+
+
+
+ When enabled, openpilot will display current road name and speed limit on the screen.
+Reboot required.
+
+
+
+ Enable Lane Priority Mode
+
+
+
+ When enabled, openpilot will use lane lines for lateral control, fallback to laneless mode automatically when lane lines probabilities are low.
+Reboot required.
+
+
Updater
diff --git a/selfdrive/ui/translations/main_zh-CHT.qm b/selfdrive/ui/translations/main_zh-CHT.qm
index 2bb956324..db957eec2 100644
Binary files a/selfdrive/ui/translations/main_zh-CHT.qm and b/selfdrive/ui/translations/main_zh-CHT.qm differ
diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts
index 5f8c18116..25bc91320 100644
--- a/selfdrive/ui/translations/main_zh-CHT.ts
+++ b/selfdrive/ui/translations/main_zh-CHT.ts
@@ -109,6 +109,13 @@
IP 地址
+
+ CarSelectionPanel
+
+ [AUTO SELECT]
+ [自動選擇]
+
+
ConfirmationDialog
@@ -657,6 +664,14 @@ This may take up to a minute.
Navigation
導航
+
+ Vehicle Model:
+ 車型:
+
+
+ [AUTO SELECT]
+ [自動選擇]
+
Setup
@@ -1055,23 +1070,53 @@ This may take up to a minute.
openpilot Longitudinal Control (Alpha)
-
+ openpilot 縱向控制(Alpha 版)
WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).
-
+ 警告:此車輛的 openpilot 縱向控制功能尚處於測試階段(Alpha 版),使用該功能將會停用自動緊急制動系統(AEB)。
On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.
-
+ 在這輛車上,openpilot 默認使用車輛內建的自適應巡航系統(ACC),而不是 openpilot 的縱向控制功能。啟用此功能可切換至 openpilot 的縱向控制模式。建議在啟用 openpilo t縱向控制 alpha 版時啟用實驗模式。
Enable Right-Hand Drive
-
+ 啟用右駕模式
Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat.
-
+ 允許 openpilot 遵守左側交通規則並在右側駕駛座進行駕駛者監控。
+
+
+ Enable ALKA
+ 啟用全時置中
+
+
+ When enabled, openpilot lateral Control will be always on when ACC MAIN is ON.
+Reboot required.
+ 啟用後,openpilot 的橫向控制將始終在 ACC MAIN 開啟時開啟。
+需要重新啟動。
+
+
+ Enable MapD
+ 啟用地圖服務
+
+
+ When enabled, openpilot will display current road name and speed limit on the screen.
+Reboot required.
+ 啟用後,openpilot 將在屏幕上顯示當前道路名稱和速限。
+需要重新啟動。
+
+
+ Enable Lane Priority Mode
+ 啟用車道優先模式
+
+
+ When enabled, openpilot will use lane lines for lateral control, fallback to laneless mode automatically when lane lines probabilities are low.
+Reboot required.
+ 啟用後,openpilot 將使用車道線進行橫向控制,在車道線的概率低時自動切換至無車道線模式。
+需要重新啟動。
diff --git a/system/clocksd/clocksd b/system/clocksd/clocksd
index 4c88b54b5..610fefc32 100755
Binary files a/system/clocksd/clocksd and b/system/clocksd/clocksd differ
diff --git a/system/hardware/base.h b/system/hardware/base.h
index 546009972..80ce8da98 100644
--- a/system/hardware/base.h
+++ b/system/hardware/base.h
@@ -34,4 +34,5 @@ public:
static bool PC() { return false; }
static bool TICI() { return false; }
static bool AGNOS() { return false; }
+ static bool EON() { return false; }
};
diff --git a/system/hardware/eon/libs/ncompress.cpython-38.so b/system/hardware/eon/libs/ncompress.cpython-38.so
new file mode 100755
index 000000000..d7807e902
Binary files /dev/null and b/system/hardware/eon/libs/ncompress.cpython-38.so differ
diff --git a/selfdrive/mapd/assets/osm-3s_v0.7.56.tar.xz b/system/hardware/eon/libs/osm-3s_v0.7.56.tar.xz
similarity index 100%
rename from selfdrive/mapd/assets/osm-3s_v0.7.56.tar.xz
rename to system/hardware/eon/libs/osm-3s_v0.7.56.tar.xz
diff --git a/system/hardware/eon/libs/spidev.cpython-38.so b/system/hardware/eon/libs/spidev.cpython-38.so
new file mode 100755
index 000000000..f6d5533fb
Binary files /dev/null and b/system/hardware/eon/libs/spidev.cpython-38.so differ
diff --git a/system/logcatd/logcatd b/system/logcatd/logcatd
index fb5f56bb5..1cca5876d 100755
Binary files a/system/logcatd/logcatd and b/system/logcatd/logcatd differ
diff --git a/system/proclogd/proclogd b/system/proclogd/proclogd
index 91d98856c..8cef066f6 100755
Binary files a/system/proclogd/proclogd and b/system/proclogd/proclogd differ
diff --git a/system/sensord/_sensord b/system/sensord/_sensord
index 1ed89b116..51feb3643 100755
Binary files a/system/sensord/_sensord and b/system/sensord/_sensord differ
diff --git a/system/ubloxd/ubloxd b/system/ubloxd/ubloxd
index 6162dcbe7..345f0ea2e 100755
Binary files a/system/ubloxd/ubloxd and b/system/ubloxd/ubloxd differ
diff --git a/third_party/acados/acados_template/__pycache__/__init__.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/__init__.cpython-38.pyc
index d075c475e..b07ec67b9 100644
Binary files a/third_party/acados/acados_template/__pycache__/__init__.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/__init__.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/acados_model.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/acados_model.cpython-38.pyc
index f61d27905..cfe31fdac 100644
Binary files a/third_party/acados/acados_template/__pycache__/acados_model.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/acados_model.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/acados_ocp.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/acados_ocp.cpython-38.pyc
index 2dbe14fca..0a9df576d 100644
Binary files a/third_party/acados/acados_template/__pycache__/acados_ocp.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/acados_ocp.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/acados_ocp_solver.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/acados_ocp_solver.cpython-38.pyc
index e6a435146..4b3413130 100644
Binary files a/third_party/acados/acados_template/__pycache__/acados_ocp_solver.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/acados_ocp_solver.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/acados_sim.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/acados_sim.cpython-38.pyc
index 7f77697db..5c00ec50f 100644
Binary files a/third_party/acados/acados_template/__pycache__/acados_sim.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/acados_sim.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/acados_sim_solver.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/acados_sim_solver.cpython-38.pyc
index b25fbbfcc..e844653f3 100644
Binary files a/third_party/acados/acados_template/__pycache__/acados_sim_solver.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/acados_sim_solver.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/builders.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/builders.cpython-38.pyc
index 9a723c889..4b1554387 100644
Binary files a/third_party/acados/acados_template/__pycache__/builders.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/builders.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/generate_c_code_constraint.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/generate_c_code_constraint.cpython-38.pyc
index 4eb4fc99b..f918b89c3 100644
Binary files a/third_party/acados/acados_template/__pycache__/generate_c_code_constraint.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/generate_c_code_constraint.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/generate_c_code_discrete_dynamics.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/generate_c_code_discrete_dynamics.cpython-38.pyc
index 4cdb7d2ef..d5ae40baf 100644
Binary files a/third_party/acados/acados_template/__pycache__/generate_c_code_discrete_dynamics.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/generate_c_code_discrete_dynamics.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/generate_c_code_explicit_ode.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/generate_c_code_explicit_ode.cpython-38.pyc
index 100a4b27f..e61815923 100644
Binary files a/third_party/acados/acados_template/__pycache__/generate_c_code_explicit_ode.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/generate_c_code_explicit_ode.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/generate_c_code_external_cost.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/generate_c_code_external_cost.cpython-38.pyc
index 3dca1a1c7..0a7be844b 100644
Binary files a/third_party/acados/acados_template/__pycache__/generate_c_code_external_cost.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/generate_c_code_external_cost.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/generate_c_code_gnsf.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/generate_c_code_gnsf.cpython-38.pyc
index c1c849ccb..15913c701 100644
Binary files a/third_party/acados/acados_template/__pycache__/generate_c_code_gnsf.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/generate_c_code_gnsf.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/generate_c_code_implicit_ode.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/generate_c_code_implicit_ode.cpython-38.pyc
index 533ce479a..39be65e9f 100644
Binary files a/third_party/acados/acados_template/__pycache__/generate_c_code_implicit_ode.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/generate_c_code_implicit_ode.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/generate_c_code_nls_cost.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/generate_c_code_nls_cost.cpython-38.pyc
index 0a72c611f..63dbbbdab 100644
Binary files a/third_party/acados/acados_template/__pycache__/generate_c_code_nls_cost.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/generate_c_code_nls_cost.cpython-38.pyc differ
diff --git a/third_party/acados/acados_template/__pycache__/utils.cpython-38.pyc b/third_party/acados/acados_template/__pycache__/utils.cpython-38.pyc
index 36eba1bbe..18c8ae4b8 100644
Binary files a/third_party/acados/acados_template/__pycache__/utils.cpython-38.pyc and b/third_party/acados/acados_template/__pycache__/utils.cpython-38.pyc differ
diff --git a/third_party/cluster/__pycache__/__init__.cpython-38.pyc b/third_party/cluster/__pycache__/__init__.cpython-38.pyc
index f6775866b..99cd778d7 100644
Binary files a/third_party/cluster/__pycache__/__init__.cpython-38.pyc and b/third_party/cluster/__pycache__/__init__.cpython-38.pyc differ
diff --git a/third_party/cluster/__pycache__/fastcluster_py.cpython-38.pyc b/third_party/cluster/__pycache__/fastcluster_py.cpython-38.pyc
index 65bcd40b0..3c084a3bc 100644
Binary files a/third_party/cluster/__pycache__/fastcluster_py.cpython-38.pyc and b/third_party/cluster/__pycache__/fastcluster_py.cpython-38.pyc differ