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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Acura|RDX 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Buick|LaCrosse 2017-19[3](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|Cadillac|Escalade 2017[3](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|Cadillac|Escalade ESV 2016[3](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| -|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| -|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| -|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| -|Chevrolet|Volt 2017-18[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Chrysler|Pacifica Hybrid 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| +|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
View- 1 FCA connector
- 1 RJ45 cable (7 ft)
- 1 comma power v2
- 1 harness box
|| |comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None|| -|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ford Q3|| -|Ford|Escape 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ford Q3|| -|Ford|Explorer 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ford Q3|| -|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ford Q3|| -|Ford|Maverick 2022-23|Co-Pilot360 Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ford Q3|| -|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J|| -|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Genesis|GV60 (Advanced Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Genesis|GV60 (Performance Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Genesis|GV70 (2.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Genesis|GV70 (3.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai M|| -|Genesis|GV80 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai M|| -|GMC|Acadia 2018[3](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|OBD-II|| -|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|GM|| -|Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Accord Hybrid 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Civic 2019-21|All|openpilot available[1](#footnotes)|0 mph|2 mph[4](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Civic 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B|| -|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Civic Hatchback 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B|| -|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|CR-V 2017-22|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|e 2020|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|HR-V 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch B|| -|Honda|Insight 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Inspire 2018|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Bosch A|| -|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Passport 2019-21|All|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Honda Nidec|| -|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|| -|Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai J|| -|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q|| -|Hyundai|Ioniq 5 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai Q|| -|Hyundai|Ioniq 5 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|| -|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| -|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai O|| -|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai I|| -|Hyundai|Palisade 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Hyundai|Santa Cruz 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D|| -|Hyundai|Santa Fe 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Hyundai|Santa Fe Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Hyundai|Sonata 2020-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Hyundai|Sonata Hybrid 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Hyundai|Tucson 2022[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Hyundai|Tucson 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Hyundai|Tucson Hybrid 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|FCA|| -|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Kia|EV6 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|| -|Kia|EV6 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai P|| -|Kia|EV6 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai L|| -|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| -|Kia|Forte 2023|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Niro EV 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Kia|Niro EV 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Niro Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai F|| -|Kia|Niro Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[1](#footnotes)|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai D|| -|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai B|| -|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai G|| -|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai E|| -|Kia|Sorento 2021-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai A|| -|Kia|Sportage 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai N|| -|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai C|| -|Kia|Stinger 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai K|| -|Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Hyundai H|| -|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|ES 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|ES Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|NX 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|NX Hybrid 2018-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Lincoln|Aviator 2021|Co-Pilot360 Plus|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ford Q3|| -|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| -|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Mazda|| -|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan B|| -|Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| -|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| -|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Nissan A|| -|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Ram|| -|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru B|| -|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru B|| -|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A|| -|Škoda|Fabia 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Škoda|Kamiq 2021[7](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Škoda|Kodiaq 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Avalon 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Avalon 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Avalon Hybrid 2019-21|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|C-HR Hybrid 2021-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Camry 2018-20|All|Stock|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Camry 2021-23|All|openpilot|0 mph[6](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Prius 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Prius Prime 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|RAV4 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Toyota|| -|Toyota|Sienna 2018-20|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|Toyota|| -|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Passat 2015-22[8](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533[10](#footnotes)|| -|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| -|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,9](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|J533|| +|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
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