mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-21 16:23:50 +08:00
lp-dp 2023-05-25T03:10:00 for EON/C2
version: lp-dp v0.9.3 for EON/C2 date: 2023-05-25T03:10:00 commit: 409000e6091be63a423d23e7940d569fed28497e
This commit is contained in:
Vehicle Researcher
@@ -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).
|
||||
|
||||
|
||||
@@ -1,3 +1,6 @@
|
||||
Version 0.9.3 (2023-06-XX)
|
||||
========================
|
||||
|
||||
Version 0.9.2 (2023-05-22)
|
||||
========================
|
||||
* New driving model
|
||||
|
||||
+2
-1
@@ -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;
|
||||
|
||||
+3
-6
@@ -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;
|
||||
}
|
||||
|
||||
Binary file not shown.
+10
-2
@@ -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;
|
||||
|
||||
Binary file not shown.
@@ -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)
|
||||
|
||||
Binary file not shown.
+1
-1
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "2023.05.19"
|
||||
#define COMMA_VERSION "2023.05.25"
|
||||
|
||||
+252
-252
@@ -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|<a href="##"><img width=2000></a>Harness Kit<br> |Video|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
|
||||
|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Acura&model=ILX 2016-19">Honda Nidec</a>||
|
||||
|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Acura&model=RDX 2016-18">Honda Nidec</a>||
|
||||
|Acura|RDX 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Acura&model=RDX 2019-22">Honda Bosch A</a>||
|
||||
|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=A3 2014-19">J533</a>||
|
||||
|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=A3 Sportback e-tron 2017-18">J533</a>||
|
||||
|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=Q2 2018">J533</a>||
|
||||
|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=Q3 2019-23">J533</a>||
|
||||
|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=RS3 2018">J533</a>||
|
||||
|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Audi&model=S3 2015-17">J533</a>||
|
||||
|Buick|LaCrosse 2017-19[<sup>3</sup>](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Buick&model=LaCrosse 2017-19">OBD-II</a>||
|
||||
|Cadillac|Escalade 2017[<sup>3</sup>](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Cadillac&model=Escalade 2017">OBD-II</a>||
|
||||
|Cadillac|Escalade ESV 2016[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Cadillac&model=Escalade ESV 2016">OBD-II</a>||
|
||||
|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Bolt EUV 2022-23">GM</a>|<a href="https://youtu.be/xvwzGMUA210" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Bolt EV 2022-23">GM</a>||
|
||||
|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Silverado 1500 2020-21">GM</a>||
|
||||
|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Trailblazer 2021-22">GM</a>||
|
||||
|Chevrolet|Volt 2017-18[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Volt 2017-18">OBD-II</a>|<a href="https://youtu.be/QeMCN_4TFfQ" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica 2017-18">FCA</a>||
|
||||
|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica 2019-20">FCA</a>||
|
||||
|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica 2021">FCA</a>||
|
||||
|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica Hybrid 2017-18">FCA</a>||
|
||||
|Chrysler|Pacifica Hybrid 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica Hybrid 2019-23">FCA</a>||
|
||||
|Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Acura&model=ILX 2016-19">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Acura&model=RDX 2016-18">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Acura|RDX 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Acura&model=RDX 2019-22">Honda Bosch A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Audi&model=A3 2014-19">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Audi&model=A3 Sportback e-tron 2017-18">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Audi&model=Q2 2018">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Audi&model=Q3 2019-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Audi&model=RS3 2018">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Audi&model=S3 2015-17">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Buick|LaCrosse 2017-19[<sup>3</sup>](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Buick&model=LaCrosse 2017-19">OBD-II connector</a><br>- 1 USB-C coupler<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Cadillac|Escalade 2017[<sup>3</sup>](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Cadillac&model=Escalade 2017">OBD-II connector</a><br>- 1 USB-C coupler<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Cadillac|Escalade ESV 2016[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Cadillac&model=Escalade ESV 2016">OBD-II connector</a><br>- 1 USB-C coupler<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Bolt EUV 2022-23">GM connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/xvwzGMUA210" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Bolt EV 2022-23">GM connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Silverado 1500 2020-21">GM connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Trailblazer 2021-22">GM connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Chevrolet|Volt 2017-18[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Chevrolet&model=Volt 2017-18">OBD-II connector</a><br>- 1 USB-C coupler<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/QeMCN_4TFfQ" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica 2017-18">FCA connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica 2019-20">FCA connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Chrysler|Pacifica 2021|All|Stock|0 mph|39 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica 2021">FCA connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica Hybrid 2017-18">FCA connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Chrysler|Pacifica Hybrid 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Chrysler&model=Pacifica Hybrid 2019-23">FCA connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|comma|body|All|openpilot|0 mph|0 mph|[](##)|[](##)|None||
|
||||
|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Bronco Sport 2021-22">Ford Q3</a>||
|
||||
|Ford|Escape 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Escape 2020-22">Ford Q3</a>||
|
||||
|Ford|Explorer 2020-22|Co-Pilot360 Assist+|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Explorer 2020-22">Ford Q3</a>||
|
||||
|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Kuga 2020-22">Ford Q3</a>||
|
||||
|Ford|Maverick 2022-23|Co-Pilot360 Assist|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Maverick 2022-23">Ford Q3</a>||
|
||||
|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G70 2018-19">Hyundai F</a>||
|
||||
|Genesis|G70 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G70 2020">Hyundai F</a>||
|
||||
|Genesis|G80 2017|All|Stock|19 mph|37 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G80 2017">Hyundai J</a>||
|
||||
|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G80 2018-19">Hyundai H</a>||
|
||||
|Genesis|G90 2017-18|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G90 2017-18">Hyundai C</a>||
|
||||
|Genesis|GV60 (Advanced Trim) 2023[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV60 (Advanced Trim) 2023">Hyundai A</a>||
|
||||
|Genesis|GV60 (Performance Trim) 2023[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV60 (Performance Trim) 2023">Hyundai K</a>||
|
||||
|Genesis|GV70 (2.5T Trim) 2022-23[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV70 (2.5T Trim) 2022-23">Hyundai L</a>||
|
||||
|Genesis|GV70 (3.5T Trim) 2022-23[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV70 (3.5T Trim) 2022-23">Hyundai M</a>||
|
||||
|Genesis|GV80 2023[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV80 2023">Hyundai M</a>||
|
||||
|GMC|Acadia 2018[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=GMC&model=Acadia 2018">OBD-II</a>|<a href="https://www.youtube.com/watch?v=0ZN6DdsBUZo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=GMC&model=Sierra 1500 2020-21">GM</a>|<a href="https://youtu.be/5HbNoBLzRwE" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|Accord 2018-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Accord 2018-22">Honda Bosch A</a>|<a href="https://www.youtube.com/watch?v=mrUwlj3Mi58" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|Accord Hybrid 2018-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Accord Hybrid 2018-22">Honda Bosch A</a>||
|
||||
|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic 2016-18">Honda Nidec</a>|<a href="https://youtu.be/-IkImTe1NYE" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|Civic 2019-21|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|2 mph[<sup>4</sup>](#footnotes)|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic 2019-21">Honda Bosch A</a>|<a href="https://www.youtube.com/watch?v=4Iz1Mz5LGF8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|Civic 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic 2022">Honda Bosch B</a>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic Hatchback 2017-21">Honda Bosch A</a>||
|
||||
|Honda|Civic Hatchback 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic Hatchback 2022">Honda Bosch B</a>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=CR-V 2015-16">Honda Nidec</a>||
|
||||
|Honda|CR-V 2017-22|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=CR-V 2017-22">Honda Bosch A</a>||
|
||||
|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=CR-V Hybrid 2017-19">Honda Bosch A</a>||
|
||||
|Honda|e 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=e 2020">Honda Bosch A</a>||
|
||||
|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Fit 2018-20">Honda Nidec</a>||
|
||||
|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Freed 2020">Honda Nidec</a>||
|
||||
|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=HR-V 2019-22">Honda Nidec</a>||
|
||||
|Honda|HR-V 2023|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=HR-V 2023">Honda Bosch B</a>||
|
||||
|Honda|Insight 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Insight 2019-22">Honda Bosch A</a>||
|
||||
|Honda|Inspire 2018|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Inspire 2018">Honda Bosch A</a>||
|
||||
|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Odyssey 2018-20">Honda Nidec</a>||
|
||||
|Honda|Passport 2019-21|All|openpilot|25 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Passport 2019-21">Honda Nidec</a>||
|
||||
|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Pilot 2016-22">Honda Nidec</a>||
|
||||
|Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Ridgeline 2017-23">Honda Nidec</a>||
|
||||
|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra 2017-19">Hyundai B</a>||
|
||||
|Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra 2021-23">Hyundai K</a>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra GT 2017-19">Hyundai E</a>||
|
||||
|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra Hybrid 2021-23">Hyundai K</a>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Genesis 2015-16">Hyundai J</a>||
|
||||
|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=i30 2017-19">Hyundai E</a>||
|
||||
|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq 5 (Southeast Asia only) 2022-23">Hyundai Q</a>||
|
||||
|Hyundai|Ioniq 5 (with HDA II) 2022-23[<sup>5</sup>](#footnotes)|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq 5 (with HDA II) 2022-23">Hyundai Q</a>||
|
||||
|Hyundai|Ioniq 5 (without HDA II) 2022-23[<sup>5</sup>](#footnotes)|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq 5 (without HDA II) 2022-23">Hyundai K</a>||
|
||||
|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Electric 2019">Hyundai C</a>||
|
||||
|Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Electric 2020">Hyundai H</a>||
|
||||
|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Hybrid 2017-19">Hyundai C</a>||
|
||||
|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Hybrid 2020-22">Hyundai H</a>||
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|32 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Plug-in Hybrid 2019">Hyundai C</a>||
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Plug-in Hybrid 2020-22">Hyundai H</a>||
|
||||
|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona 2020">Hyundai B</a>||
|
||||
|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona Electric 2018-21">Hyundai G</a>||
|
||||
|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona Electric 2022">Hyundai O</a>||
|
||||
|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona Hybrid 2020">Hyundai I</a>|<a href="https://youtu.be/0dwpAHiZgFo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Palisade 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Palisade 2020-22">Hyundai H</a>|<a href="https://youtu.be/TAnDqjF4fDY?t=456" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Santa Cruz 2022-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Cruz 2022-23">Hyundai N</a>||
|
||||
|Hyundai|Santa Fe 2019-20|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe 2019-20">Hyundai D</a>||
|
||||
|Hyundai|Santa Fe 2021-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe 2021-22">Hyundai L</a>|<a href="https://youtu.be/VnHzSTygTS4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Santa Fe Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe Hybrid 2022">Hyundai L</a>||
|
||||
|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe Plug-in Hybrid 2022">Hyundai L</a>||
|
||||
|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Sonata 2018-19">Hyundai E</a>||
|
||||
|Hyundai|Sonata 2020-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Sonata 2020-23">Hyundai A</a>|<a href="https://www.youtube.com/watch?v=ix63r9kE3Fw" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Sonata Hybrid 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Sonata Hybrid 2020-22">Hyundai A</a>||
|
||||
|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson 2021">Hyundai L</a>||
|
||||
|Hyundai|Tucson 2022[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson 2022">Hyundai N</a>||
|
||||
|Hyundai|Tucson 2023[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson 2023">Hyundai N</a>||
|
||||
|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson Diesel 2019">Hyundai L</a>||
|
||||
|Hyundai|Tucson Hybrid 2022-23[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson Hybrid 2022-23">Hyundai N</a>||
|
||||
|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Veloster 2019-20">Hyundai E</a>||
|
||||
|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Jeep&model=Grand Cherokee 2016-18">FCA</a>|<a href="https://www.youtube.com/watch?v=eLR9o2JkuRk" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Jeep&model=Grand Cherokee 2019-21">FCA</a>|<a href="https://www.youtube.com/watch?v=jBe4lWnRSu4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Ceed 2019">Hyundai E</a>||
|
||||
|Kia|EV6 (Southeast Asia only) 2022-23[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=EV6 (Southeast Asia only) 2022-23">Hyundai P</a>||
|
||||
|Kia|EV6 (with HDA II) 2022-23[<sup>5</sup>](#footnotes)|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=EV6 (with HDA II) 2022-23">Hyundai P</a>||
|
||||
|Kia|EV6 (without HDA II) 2022-23[<sup>5</sup>](#footnotes)|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=EV6 (without HDA II) 2022-23">Hyundai L</a>||
|
||||
|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Forte 2019-21">Hyundai G</a>||
|
||||
|Kia|Forte 2023|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Forte 2023">Hyundai E</a>||
|
||||
|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=K5 2021-22">Hyundai A</a>||
|
||||
|Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=K5 Hybrid 2020">Hyundai A</a>||
|
||||
|Kia|Niro EV 2019|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2019">Hyundai H</a>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Niro EV 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2020">Hyundai F</a>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Niro EV 2021|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2021">Hyundai C</a>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Niro EV 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2022">Hyundai H</a>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Niro EV 2023[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2023">Hyundai A</a>||
|
||||
|Kia|Niro Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Hybrid 2021-22">Hyundai F</a>||
|
||||
|Kia|Niro Hybrid 2023[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Hybrid 2023">Hyundai A</a>||
|
||||
|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[<sup>1</sup>](#footnotes)|10 mph|32 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Plug-in Hybrid 2018-19">Hyundai C</a>||
|
||||
|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|32 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Plug-in Hybrid 2020">Hyundai D</a>||
|
||||
|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Optima 2017">Hyundai B</a>||
|
||||
|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Optima 2019-20">Hyundai G</a>||
|
||||
|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Seltos 2021">Hyundai A</a>||
|
||||
|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2018">Hyundai C</a>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2019">Hyundai E</a>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Sorento 2021-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2021-23">Hyundai K</a>||
|
||||
|Kia|Sorento Plug-in Hybrid 2022-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento Plug-in Hybrid 2022-23">Hyundai A</a>||
|
||||
|Kia|Sportage 2023[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sportage 2023">Hyundai N</a>||
|
||||
|Kia|Sportage Hybrid 2023[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sportage Hybrid 2023">Hyundai N</a>||
|
||||
|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Stinger 2018-20">Hyundai C</a>|<a href="https://www.youtube.com/watch?v=MJ94qoofYw0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Stinger 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Stinger 2022">Hyundai K</a>||
|
||||
|Kia|Telluride 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Telluride 2020-22">Hyundai H</a>||
|
||||
|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=CT Hybrid 2017-18">Toyota</a>||
|
||||
|Lexus|ES 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES 2017-18">Toyota</a>||
|
||||
|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES 2019-22">Toyota</a>||
|
||||
|Lexus|ES Hybrid 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES Hybrid 2017-18">Toyota</a>||
|
||||
|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES Hybrid 2019-23">Toyota</a>|<a href="https://youtu.be/BZ29osRVJeg?t=12" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=IS 2017-19">Toyota</a>||
|
||||
|Lexus|NX 2018-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX 2018-19">Toyota</a>||
|
||||
|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX 2020-21">Toyota</a>||
|
||||
|Lexus|NX Hybrid 2018-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX Hybrid 2018-19">Toyota</a>||
|
||||
|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX Hybrid 2020-21">Toyota</a>||
|
||||
|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RC 2018-20">Toyota</a>||
|
||||
|Lexus|RX 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2016">Toyota</a>||
|
||||
|Lexus|RX 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2017-19">Toyota</a>||
|
||||
|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2020-22">Toyota</a>||
|
||||
|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX Hybrid 2016">Toyota</a>||
|
||||
|Lexus|RX Hybrid 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX Hybrid 2017-19">Toyota</a>||
|
||||
|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX Hybrid 2020-21">Toyota</a>||
|
||||
|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=UX Hybrid 2019-22">Toyota</a>||
|
||||
|Lincoln|Aviator 2021|Co-Pilot360 Plus|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Lincoln&model=Aviator 2021">Ford Q3</a>||
|
||||
|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=MAN&model=eTGE 2020-23">J533</a>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=MAN&model=TGE 2017-23">J533</a>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Mazda&model=CX-5 2022-23">Mazda</a>||
|
||||
|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Mazda&model=CX-9 2021-23">Mazda</a>|<a href="https://youtu.be/dA3duO4a0O4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Altima 2019-20">Nissan B</a>||
|
||||
|Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Leaf 2018-23">Nissan A</a>|<a href="https://youtu.be/vaMbtAh_0cY" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Rogue 2018-20">Nissan A</a>||
|
||||
|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=X-Trail 2017">Nissan A</a>||
|
||||
|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Ram&model=1500 2019-23">Ram</a>||
|
||||
|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=SEAT&model=Ateca 2018">J533</a>||
|
||||
|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=SEAT&model=Leon 2014-20">J533</a>||
|
||||
|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Ascent 2019-21">Subaru A</a>||
|
||||
|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Crosstrek 2018-19">Subaru A</a>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Crosstrek 2020-23">Subaru A</a>||
|
||||
|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Forester 2019-21">Subaru A</a>||
|
||||
|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Impreza 2017-19">Subaru A</a>||
|
||||
|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Impreza 2020-22">Subaru A</a>||
|
||||
|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Legacy 2020-22">Subaru B</a>||
|
||||
|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Outback 2020-22">Subaru B</a>||
|
||||
|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=XV 2018-19">Subaru A</a>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=XV 2020-21">Subaru A</a>||
|
||||
|Škoda|Fabia 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Fabia 2022-23">J533</a>[<sup>10</sup>](#footnotes)||
|
||||
|Škoda|Kamiq 2021[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Kamiq 2021">J533</a>[<sup>10</sup>](#footnotes)||
|
||||
|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Karoq 2019-21">J533</a>||
|
||||
|Škoda|Kodiaq 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Kodiaq 2017-23">J533</a>||
|
||||
|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Octavia 2015, 2018-19">J533</a>||
|
||||
|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Octavia RS 2016">J533</a>||
|
||||
|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Scala 2020">J533</a>[<sup>10</sup>](#footnotes)||
|
||||
|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Superb 2015-22">J533</a>||
|
||||
|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Alphard 2019-20">Toyota</a>||
|
||||
|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Alphard Hybrid 2021">Toyota</a>||
|
||||
|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2016">Toyota</a>||
|
||||
|Toyota|Avalon 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2017-18">Toyota</a>||
|
||||
|Toyota|Avalon 2019-21|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2019-21">Toyota</a>||
|
||||
|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2022">Toyota</a>||
|
||||
|Toyota|Avalon Hybrid 2019-21|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon Hybrid 2019-21">Toyota</a>||
|
||||
|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon Hybrid 2022">Toyota</a>||
|
||||
|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR 2017-20">Toyota</a>||
|
||||
|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR 2021">Toyota</a>||
|
||||
|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR Hybrid 2017-20">Toyota</a>||
|
||||
|Toyota|C-HR Hybrid 2021-22|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR Hybrid 2021-22">Toyota</a>||
|
||||
|Toyota|Camry 2018-20|All|Stock|0 mph[<sup>6</sup>](#footnotes)|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry 2018-20">Toyota</a>|<a href="https://www.youtube.com/watch?v=fkcjviZY9CM" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Camry 2021-23|All|openpilot|0 mph[<sup>6</sup>](#footnotes)|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry 2021-23">Toyota</a>||
|
||||
|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry Hybrid 2018-20">Toyota</a>|<a href="https://www.youtube.com/watch?v=Q2DYY0AWKgk" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry Hybrid 2021-23">Toyota</a>||
|
||||
|Toyota|Corolla 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla 2017-19">Toyota</a>||
|
||||
|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla 2020-22">Toyota</a>|<a href="https://www.youtube.com/watch?v=_66pXk0CBYA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Cross (Non-US only) 2020-23">Toyota</a>||
|
||||
|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Cross Hybrid (Non-US only) 2020-22">Toyota</a>||
|
||||
|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Hatchback 2019-22">Toyota</a>|<a href="https://www.youtube.com/watch?v=_66pXk0CBYA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Hybrid 2020-22">Toyota</a>||
|
||||
|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Hybrid (Non-US only) 2020-23">Toyota</a>||
|
||||
|Toyota|Highlander 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander 2017-19">Toyota</a>|<a href="https://www.youtube.com/watch?v=0wS0wXSLzoo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander 2020-23">Toyota</a>||
|
||||
|Toyota|Highlander Hybrid 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander Hybrid 2017-19">Toyota</a>||
|
||||
|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander Hybrid 2020-23">Toyota</a>||
|
||||
|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Mirai 2021">Toyota</a>||
|
||||
|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius 2016">Toyota</a>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Prius 2017-20|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius 2017-20">Toyota</a>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius 2021-22">Toyota</a>|<a href="https://www.youtube.com/watch?v=J58TvCpUd4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Prius Prime 2017-20|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius Prime 2017-20">Toyota</a>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius Prime 2021-22">Toyota</a>|<a href="https://www.youtube.com/watch?v=J58TvCpUd4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius v 2017">Toyota</a>||
|
||||
|Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2016">Toyota</a>||
|
||||
|Toyota|RAV4 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2017-18">Toyota</a>||
|
||||
|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2019-21">Toyota</a>|<a href="https://www.youtube.com/watch?v=wJxjDd42gGA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2022">Toyota</a>||
|
||||
|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2016">Toyota</a>|<a href="https://youtu.be/LhT5VzJVfNI?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2017-18">Toyota</a>|<a href="https://youtu.be/LhT5VzJVfNI?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2019-21">Toyota</a>||
|
||||
|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2022">Toyota</a>|<a href="https://youtu.be/U0nH9cnrFB0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Sienna 2018-20|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Sienna 2018-20">Toyota</a>|<a href="https://www.youtube.com/watch?v=q1UPOo4Sh68" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Arteon 2018-22">J533</a>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Arteon eHybrid 2020-22">J533</a>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Arteon R 2020-22">J533</a>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Atlas 2018-23">J533</a>||
|
||||
|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Atlas Cross Sport 2021-22">J533</a>||
|
||||
|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=California 2021">J533</a>||
|
||||
|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Caravelle 2020">J533</a>||
|
||||
|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=CC 2018-22">J533</a>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Crafter 2017-23">J533</a>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=e-Crafter 2018-23">J533</a>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=e-Golf 2014-20">J533</a>||
|
||||
|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf 2015-20">J533</a>||
|
||||
|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf Alltrack 2015-19">J533</a>||
|
||||
|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf GTD 2015-20">J533</a>||
|
||||
|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf GTE 2015-20">J533</a>||
|
||||
|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf GTI 2015-21">J533</a>||
|
||||
|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf R 2015-19">J533</a>||
|
||||
|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf SportsVan 2015-20">J533</a>||
|
||||
|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Grand California 2019-23">J533</a>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Jetta 2018-22">J533</a>||
|
||||
|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Jetta GLI 2021-22">J533</a>||
|
||||
|Volkswagen|Passat 2015-22[<sup>8</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Passat 2015-22">J533</a>||
|
||||
|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Passat Alltrack 2015-22">J533</a>||
|
||||
|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Passat GTE 2015-22">J533</a>||
|
||||
|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Polo 2018-23">J533</a>[<sup>10</sup>](#footnotes)||
|
||||
|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Polo GTI 2018-23">J533</a>[<sup>10</sup>](#footnotes)||
|
||||
|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=T-Cross 2021">J533</a>[<sup>10</sup>](#footnotes)||
|
||||
|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=T-Roc 2021">J533</a>[<sup>10</sup>](#footnotes)||
|
||||
|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Taos 2022">J533</a>||
|
||||
|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Teramont 2018-22">J533</a>||
|
||||
|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Teramont Cross Sport 2021-22">J533</a>||
|
||||
|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Teramont X 2021-22">J533</a>||
|
||||
|Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Tiguan 2018-23">J533</a>||
|
||||
|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Tiguan eHybrid 2021-23">J533</a>||
|
||||
|Volkswagen|Touran 2017|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Touran 2017">J533</a>||
|
||||
|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Bronco Sport 2021-22">Ford Q3 connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Ford|Escape 2020-22|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Escape 2020-22">Ford Q3 connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Ford|Explorer 2020-22|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Explorer 2020-22">Ford Q3 connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Kuga 2020-22">Ford Q3 connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Ford|Maverick 2022-23|Co-Pilot360 Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Ford&model=Maverick 2022-23">Ford Q3 connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G70 2018-19">Hyundai F connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Genesis|G70 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G70 2020">Hyundai F connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Genesis|G80 2017|All|Stock|19 mph|37 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G80 2017">Hyundai J connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G80 2018-19">Hyundai H connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Genesis|G90 2017-18|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=G90 2017-18">Hyundai C connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Genesis|GV60 (Advanced Trim) 2023[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV60 (Advanced Trim) 2023">Hyundai A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Genesis|GV60 (Performance Trim) 2023[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV60 (Performance Trim) 2023">Hyundai K connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Genesis|GV70 (2.5T Trim) 2022-23[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV70 (2.5T Trim) 2022-23">Hyundai L connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Genesis|GV70 (3.5T Trim) 2022-23[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV70 (3.5T Trim) 2022-23">Hyundai M connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Genesis|GV80 2023[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Genesis&model=GV80 2023">Hyundai M connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|GMC|Acadia 2018[<sup>3</sup>](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=GMC&model=Acadia 2018">OBD-II connector</a><br>- 1 USB-C coupler<br>- 1 long OBD-C cable</sub></details>|<a href="https://www.youtube.com/watch?v=0ZN6DdsBUZo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=GMC&model=Sierra 1500 2020-21">GM connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/5HbNoBLzRwE" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|Accord 2018-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Accord 2018-22">Honda Bosch A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=mrUwlj3Mi58" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|Accord Hybrid 2018-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Accord Hybrid 2018-22">Honda Bosch A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic 2016-18">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/-IkImTe1NYE" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|Civic 2019-21|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|2 mph[<sup>4</sup>](#footnotes)|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic 2019-21">Honda Bosch A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=4Iz1Mz5LGF8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|Civic 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic 2022">Honda Bosch B connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic Hatchback 2017-21">Honda Bosch A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|Civic Hatchback 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Civic Hatchback 2022">Honda Bosch B connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=CR-V 2015-16">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|CR-V 2017-22|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=CR-V 2017-22">Honda Bosch A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|CR-V Hybrid 2017-19|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=CR-V Hybrid 2017-19">Honda Bosch A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|e 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=e 2020">Honda Bosch A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Fit 2018-20">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Freed 2020">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|HR-V 2019-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=HR-V 2019-22">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|HR-V 2023|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=HR-V 2023">Honda Bosch B connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|Insight 2019-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Insight 2019-22">Honda Bosch A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|Inspire 2018|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Inspire 2018">Honda Bosch A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|Odyssey 2018-20|Honda Sensing|openpilot|25 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Odyssey 2018-20">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|Passport 2019-22|All|openpilot|25 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Passport 2019-22">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Pilot 2016-22">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Honda&model=Ridgeline 2017-23">Honda Nidec connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Elantra 2017-19|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra 2017-19">Hyundai B connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra 2021-23">Hyundai K connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra GT 2017-19">Hyundai E connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Elantra Hybrid 2021-23">Hyundai K connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/_EdYQtV52-c" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Genesis 2015-16">Hyundai J connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=i30 2017-19">Hyundai E connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq 5 (Southeast Asia only) 2022-23">Hyundai Q connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Ioniq 5 (with HDA II) 2022-23[<sup>5</sup>](#footnotes)|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq 5 (with HDA II) 2022-23">Hyundai Q connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Ioniq 5 (without HDA II) 2022-23[<sup>5</sup>](#footnotes)|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq 5 (without HDA II) 2022-23">Hyundai K connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Electric 2019">Hyundai C connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Ioniq Electric 2020|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Electric 2020">Hyundai H connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Hybrid 2017-19">Hyundai C connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Hybrid 2020-22">Hyundai H connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|32 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Plug-in Hybrid 2019">Hyundai C connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Ioniq Plug-in Hybrid 2020-22">Hyundai H connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona 2020">Hyundai B connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona Electric 2018-21">Hyundai G connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Kona Electric 2022|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona Electric 2022">Hyundai O connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Kona Hybrid 2020">Hyundai I connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/0dwpAHiZgFo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Palisade 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Palisade 2020-22">Hyundai H connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/TAnDqjF4fDY?t=456" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Santa Cruz 2022-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Cruz 2022-23">Hyundai N connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Santa Fe 2019-20|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe 2019-20">Hyundai D connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Santa Fe 2021-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe 2021-22">Hyundai L connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/VnHzSTygTS4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Santa Fe Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe Hybrid 2022">Hyundai L connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Santa Fe Plug-in Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Santa Fe Plug-in Hybrid 2022">Hyundai L connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Sonata 2018-19">Hyundai E connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Sonata 2020-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Sonata 2020-23">Hyundai A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=ix63r9kE3Fw" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Hyundai|Sonata Hybrid 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Sonata Hybrid 2020-22">Hyundai A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson 2021">Hyundai L connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Tucson 2022[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson 2022">Hyundai N connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Tucson 2023[<sup>5</sup>](#footnotes)|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson 2023">Hyundai N connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson Diesel 2019">Hyundai L connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Tucson Hybrid 2022-23[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Tucson Hybrid 2022-23">Hyundai N connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Hyundai&model=Veloster 2019-20">Hyundai E connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Jeep&model=Grand Cherokee 2016-18">FCA connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=eLR9o2JkuRk" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Jeep&model=Grand Cherokee 2019-21">FCA connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=jBe4lWnRSu4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Ceed 2019">Hyundai E connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|EV6 (Southeast Asia only) 2022-23[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=EV6 (Southeast Asia only) 2022-23">Hyundai P connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|EV6 (with HDA II) 2022-23[<sup>5</sup>](#footnotes)|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=EV6 (with HDA II) 2022-23">Hyundai P connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|EV6 (without HDA II) 2022-23[<sup>5</sup>](#footnotes)|Highway Driving Assist|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=EV6 (without HDA II) 2022-23">Hyundai L connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Forte 2019-21">Hyundai G connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Forte 2023|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Forte 2023">Hyundai E connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|K5 2021-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=K5 2021-22">Hyundai A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|K5 Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=K5 Hybrid 2020">Hyundai A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Niro EV 2019|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2019">Hyundai H connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Niro EV 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2020">Hyundai F connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Niro EV 2021|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2021">Hyundai C connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Niro EV 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2022">Hyundai H connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Niro EV 2023[<sup>5</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro EV 2023">Hyundai A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Niro Hybrid 2021-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Hybrid 2021-22">Hyundai F connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Niro Hybrid 2023[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Hybrid 2023">Hyundai A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Niro Plug-in Hybrid 2018-19|All|openpilot available[<sup>1</sup>](#footnotes)|10 mph|32 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Plug-in Hybrid 2018-19">Hyundai C connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Niro Plug-in Hybrid 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|32 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Niro Plug-in Hybrid 2020">Hyundai D connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Optima 2017">Hyundai B connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Optima 2019-20">Hyundai G connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Seltos 2021">Hyundai A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Sorento 2018|Advanced Smart Cruise Control|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2018">Hyundai C connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2019">Hyundai E connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Sorento 2021-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento 2021-23">Hyundai K connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Sorento Plug-in Hybrid 2022-23[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sorento Plug-in Hybrid 2022-23">Hyundai A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Sportage 2023[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sportage 2023">Hyundai N connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Sportage Hybrid 2023[<sup>5</sup>](#footnotes)|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Sportage Hybrid 2023">Hyundai N connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Stinger 2018-20">Hyundai C connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=MJ94qoofYw0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Kia|Stinger 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Stinger 2022">Hyundai K connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Kia|Telluride 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Kia&model=Telluride 2020-22">Hyundai H connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|CT Hybrid 2017-18|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=CT Hybrid 2017-18">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|ES 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES 2017-18">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|ES 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES 2019-22">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|ES Hybrid 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES Hybrid 2017-18">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|ES Hybrid 2019-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=ES Hybrid 2019-23">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/BZ29osRVJeg?t=12" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=IS 2017-19">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|NX 2018-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX 2018-19">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX 2020-21">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|NX Hybrid 2018-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX Hybrid 2018-19">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|NX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=NX Hybrid 2020-21">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|RC 2018-20|All|Stock|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RC 2018-20">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|RX 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2016">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|RX 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2017-19">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|RX 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX 2020-22">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|RX Hybrid 2016|Lexus Safety System+|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX Hybrid 2016">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|RX Hybrid 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX Hybrid 2017-19">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|RX Hybrid 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=RX Hybrid 2020-21">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lexus|UX Hybrid 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lexus&model=UX Hybrid 2019-22">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Lincoln|Aviator 2021|Co-Pilot360 Plus|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Lincoln&model=Aviator 2021">Ford Q3 connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=MAN&model=eTGE 2020-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=MAN&model=TGE 2017-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Mazda|CX-5 2022-23|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Mazda&model=CX-5 2022-23">Mazda connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Mazda&model=CX-9 2021-23">Mazda connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/dA3duO4a0O4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Altima 2019-20">Nissan B connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Nissan|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Leaf 2018-23">Nissan A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/vaMbtAh_0cY" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=Rogue 2018-20">Nissan A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Nissan&model=X-Trail 2017">Nissan A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Ram|1500 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Ram&model=1500 2019-23">Ram connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=SEAT&model=Ateca 2018">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=SEAT&model=Leon 2014-20">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Subaru|Ascent 2019-21|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Ascent 2019-21">Subaru A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Crosstrek 2018-19">Subaru A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Crosstrek 2020-23">Subaru A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Forester 2019-21">Subaru A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Impreza 2017-19">Subaru A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Impreza 2020-22">Subaru A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Legacy 2020-22">Subaru B connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Subaru|Outback 2020-22|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=Outback 2020-22">Subaru B connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Subaru|XV 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=XV 2018-19">Subaru A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Subaru|XV 2020-21|EyeSight Driver Assistance|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Subaru&model=XV 2020-21">Subaru A connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Škoda|Fabia 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Fabia 2022-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>[<sup>10</sup>](#footnotes)||
|
||||
|Škoda|Kamiq 2021[<sup>7</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Kamiq 2021">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>[<sup>10</sup>](#footnotes)||
|
||||
|Škoda|Karoq 2019-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Karoq 2019-21">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Škoda|Kodiaq 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Kodiaq 2017-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Škoda|Octavia 2015, 2018-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Octavia 2015, 2018-19">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Škoda|Octavia RS 2016|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Octavia RS 2016">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Škoda|Scala 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Scala 2020">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>[<sup>10</sup>](#footnotes)||
|
||||
|Škoda|Superb 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Škoda&model=Superb 2015-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Alphard 2019-20">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Alphard Hybrid 2021">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2016">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Avalon 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2017-18">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Avalon 2019-21|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2019-21">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Avalon 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon 2022">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Avalon Hybrid 2019-21|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon Hybrid 2019-21">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Avalon Hybrid 2022|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Avalon Hybrid 2022">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|C-HR 2017-20|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR 2017-20">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|C-HR 2021|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR 2021">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR Hybrid 2017-20">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|C-HR Hybrid 2021-22|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=C-HR Hybrid 2021-22">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Camry 2018-20|All|Stock|0 mph[<sup>6</sup>](#footnotes)|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry 2018-20">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=fkcjviZY9CM" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Camry 2021-23|All|openpilot|0 mph[<sup>6</sup>](#footnotes)|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry 2021-23">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry Hybrid 2018-20">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=Q2DYY0AWKgk" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Camry Hybrid 2021-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Camry Hybrid 2021-23">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Corolla 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla 2017-19">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla 2020-22">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=_66pXk0CBYA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Cross (Non-US only) 2020-23">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Cross Hybrid (Non-US only) 2020-22">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Hatchback 2019-22">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=_66pXk0CBYA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Hybrid 2020-22">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Corolla Hybrid (Non-US only) 2020-23">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Highlander 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander 2017-19">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=0wS0wXSLzoo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander 2020-23">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Highlander Hybrid 2017-19|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander Hybrid 2017-19">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Highlander Hybrid 2020-23">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Mirai 2021">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius 2016">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Prius 2017-20|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius 2017-20">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius 2021-22">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=J58TvCpUd4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Prius Prime 2017-20|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius Prime 2017-20">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=8zopPJI8XQ0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius Prime 2021-22">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=J58TvCpUd4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Prius v 2017">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2016">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|RAV4 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2017-18">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2019-21">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=wJxjDd42gGA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 2022">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2016">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/LhT5VzJVfNI?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[<sup>2</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2017-18">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/LhT5VzJVfNI?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2019-21">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>||
|
||||
|Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=RAV4 Hybrid 2022">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://youtu.be/U0nH9cnrFB0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Toyota|Sienna 2018-20|All|openpilot available[<sup>2</sup>](#footnotes)|19 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Toyota&model=Sienna 2018-20">Toyota connector</a><br>- 1 RJ45 cable (7 ft)<br>- 1 comma power v2<br>- 1 harness box</sub></details>|<a href="https://www.youtube.com/watch?v=q1UPOo4Sh68" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Arteon 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Arteon 2018-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Arteon eHybrid 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Arteon eHybrid 2020-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Arteon R 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Arteon R 2020-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Atlas 2018-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Atlas Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Atlas Cross Sport 2021-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|California 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=California 2021">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Caravelle 2020">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=CC 2018-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Crafter 2017-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=e-Crafter 2018-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=e-Golf 2014-20">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf 2015-20">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf Alltrack 2015-19">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf GTD 2015-20">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf GTE 2015-20">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf GTI 2015-21">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf R 2015-19">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Golf SportsVan 2015-20">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Grand California 2019-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||
|Volkswagen|Jetta 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Jetta 2018-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Jetta GLI 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Jetta GLI 2021-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Passat 2015-22[<sup>8</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Passat 2015-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Passat Alltrack 2015-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Passat GTE 2015-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Polo 2018-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>[<sup>10</sup>](#footnotes)||
|
||||
|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Polo GTI 2018-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>[<sup>10</sup>](#footnotes)||
|
||||
|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=T-Cross 2021">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>[<sup>10</sup>](#footnotes)||
|
||||
|Volkswagen|T-Roc 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=T-Roc 2021">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>[<sup>10</sup>](#footnotes)||
|
||||
|Volkswagen|Taos 2022|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Taos 2022">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Teramont 2018-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Teramont Cross Sport 2021-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Teramont X 2021-22">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Tiguan 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Tiguan 2018-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Tiguan eHybrid 2021-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|Volkswagen|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,9</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>View</summary><sub>- 1 <a href="https://comma.ai/shop/comma-three.html?make=Volkswagen&model=Touran 2016-23">J533 connector</a><br>- 1 USB-C coupler<br>- 1 harness box<br>- 1 long OBD-C cable</sub></details>||
|
||||
|
||||
<a id="footnotes"></a>
|
||||
<sup>1</sup>Experimental openpilot longitudinal control is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`. <br />
|
||||
|
||||
+9
-4
@@ -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
|
||||
|
||||
Binary file not shown.
Binary file not shown.
+1
-1
@@ -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,
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -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):
|
||||
|
||||
@@ -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
|
||||
|
||||
+5
-4
@@ -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()
|
||||
|
||||
+18
-8
@@ -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
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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
|
||||
|
||||
@@ -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""
|
||||
|
||||
@@ -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__":
|
||||
|
||||
Binary file not shown.
@@ -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',
|
||||
],
|
||||
},
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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'<a href="https://comma.ai/shop/comma-three.html?make={self.make}&model={model_years}">{harness_col}</a>'
|
||||
harness_connector = f'- 1 <a href="https://comma.ai/shop/comma-three.html?make={self.make}&model={model_years}">{harness_col} connector</a>'
|
||||
harness_parts = '<br>'.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'<details><summary>View</summary><sub>{harness_connector}<br>{harness_parts}</sub></details>'
|
||||
|
||||
self.row = {
|
||||
self.row: Dict[Enum, Union[str, Star]] = {
|
||||
Column.MAKE: self.make,
|
||||
Column.MODEL: self.model,
|
||||
Column.PACKAGE: self.package,
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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),
|
||||
]
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -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'],
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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]]] = {
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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 = {
|
||||
|
||||
@@ -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)]
|
||||
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
@@ -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]]] = {
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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,
|
||||
})
|
||||
|
||||
@@ -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,
|
||||
})
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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")),
|
||||
},
|
||||
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -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_
|
||||
@@ -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)
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
]
|
||||
}
|
||||
}
|
||||
+103
@@ -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_
|
||||
@@ -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)
|
||||
@@ -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
|
||||
]
|
||||
}
|
||||
}
|
||||
+103
@@ -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_
|
||||
@@ -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)
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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()
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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]
|
||||
|
||||
Binary file not shown.
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
Binary file not shown.
@@ -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);
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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.
|
||||
@@ -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"
|
||||
}
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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 `<restriction-value> @ (<condition>)` 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]
|
||||
@@ -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, [])
|
||||
@@ -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"
|
||||
}
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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)
|
||||
File diff suppressed because it is too large
Load Diff
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user