diff --git a/.gitignore b/.gitignore index 38b208e41c..47d1a66875 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,7 @@ .ipynb_checkpoints .idea model2.png +a.out *.DSYM *.d @@ -28,3 +29,4 @@ selfdrive/proclogd/proclogd selfdrive/ui/ui /src/ +one diff --git a/README.md b/README.md index 7ef940b6ce..dbd0c62017 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,15 @@ The openpilot codebase has been written to be concise and enable rapid prototypi Here are [some](https://www.youtube.com/watch?v=9OwTJFuDI7g) [videos](https://www.youtube.com/watch?v=64Wvt5pYQmE) [of](https://www.youtube.com/watch?v=6IW7Nejsr3A) [it](https://www.youtube.com/watch?v=-VN1YcC83nA) [running](https://www.youtube.com/watch?v=EQJZvVeihZk). And a really cool [tutorial](https://www.youtube.com/watch?v=PwOnsT2UW5o). +Community +------ + +openpilot is supported by [comma.ai](https://comma.ai/) + +We have a [Twitter you should follow](https://twitter.com/comma_ai). + +Also, we have a 3500+ person [community on slack](https://slack.comma.ai). + Hardware ------ @@ -19,49 +28,75 @@ Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` dur Supported Cars ------ -- Acura ILX 2016 with AcuraWatch Plus - - Due to use of the cruise control for gas, it can only be enabled above 25 mph +### Honda + Acura ### -- Honda Civic 2016-2018 with Honda Sensing +- Honda Accord 2018 with Honda Sensing (alpha!) + - Uses stock Honda Sensing for longitudinal control + +- Honda Civic 2016+ with Honda Sensing - Due to limitations in steering firmware, steering is disabled below 12 mph - Note that the hatchback model is not supported +- Honda Civic Hatchback 2017+ with Honda Sensing (alpha!) + - Due to limitations in steering firmware, steering is disabled below 12 mph + - Uses stock Honda Sensing for longitudinal control + +- Honda CR-V 2017-2018 with Honda Sensing (alpha!) + - Due to limitations in steering firmware, steering is disabled below 12 mph + - Uses stock Honda Sensing for longitudinal control + - Honda CR-V Touring 2015-2016 - Can only be enabled above 25 mph - Honda Odyssey 2018 with Honda Sensing (alpha!) - Can only be enabled above 25 mph -- Acura RDX 2018 with AcuraWatch Plus (alpha!) - - Can only be enabled above 25 mph - - Honda Pilot 2017 with Honda Sensing (alpha!) - Can only be enabled above 27 mph - Honda Ridgeline 2017 with Honda Sensing (alpha!) - Can only be enabled above 27 mph +- Acura ILX 2016 with AcuraWatch Plus + - Due to use of the cruise control for gas, it can only be enabled above 25 mph + +- Acura RDX 2018 with AcuraWatch Plus (alpha!) + - Can only be enabled above 25 mph + +### Toyota + Lexus ### + - Toyota RAV-4 2016+ non-hybrid with TSS-P - By default it uses stock Toyota ACC for longitudinal control - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) and can be enabled above 20 mph -- Toyota Prius 2017 (alpha!) +- Toyota Prius 2017+ - By default it uses stock Toyota ACC for longitudinal control - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Prius_.28for_openpilot.29) - Lateral control needs improvements -- Toyota RAV-4 2017 hybrid (alpha!) +- Toyota RAV-4 2017+ hybrid - By default it uses stock Toyota ACC for longitudinal control - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) and can do stop and go -- Toyota Corolla 2017 (alpha!) +- Toyota Corolla 2017+ - By default it uses stock Toyota ACC for longitudinal control - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Corolla_.28for_openpilot.29) and can be enabled above 20 mph -- Lexus RX 2017 hybrid (alpha!) +- Lexus RX 2017+ hybrid (alpha!) - By default it uses stock Lexus ACC for longitudinal control - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Lexus_RX_hybrid) +### GM (Chevrolet + Cadillac) ### + +- Chevrolet Volt Premier 2017+ + - Driver Confidence II package (adaptive cruise control) required + - Can only be enabled above 18 mph + - Read the [installation guide](https://www.zoneos.com/volt.htm) + +- Cadillac CT6 + - Uses stock ACC for longitudinal control + - Requires multiple panda for proxying the ASCMs + In Progress Cars ------ - All TSS-P Toyota with Steering Assist. @@ -71,12 +106,19 @@ In Progress Cars - 'All-Speed Range Dynamic Radar Cruise Control' is required to enable stop-and-go. Only the GS, GSH, GS, F, RX, RXH, LX, NX, NXH, LC, LCH, LS, LSH have this option. - Even though the LX have TSS-P, it does not have Steering Assist and is not supported. -Community WIP Cars +Community Maintained Cars ------ -- [Chevy Volt 2016-2018 Premier with Driver Confidence II](https://github.com/commaai/openpilot/pull/104) +- [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/246) -- [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145) +How can I add support for my car? +------ + +If your car has adaptive cruise control and lane keep assist, you are in luck. Using a [panda](https://panda.comma.ai) and [cabana](https://community.comma.ai/cabana/), you can understand how to make your car drive by wire. + +We've written a [porting guide](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) for Toyota that might help you after you have the basics figured out. + +Sadly, BMW, Audi, Volvo, and Mercedes all use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) and are unlikely to be supported any time soon. We also put time into a Ford port, but the steering has a 10 second cutout limitation that makes it unusable. Directory structure ------ @@ -95,6 +137,7 @@ Directory structure - debug -- Tools to help you debug and do car ports - logcatd -- Android logcat as a service - loggerd -- Logger and uploader of car data + - orbd -- Service generating ORB features from road camera - proclogd -- Logs information from proc - sensord -- IMU / GPS interface code - test/plant -- Car simulator running code through virtual maneuvers @@ -117,13 +160,6 @@ The results are written to `selfdrive/test/plant/out/index.html` More extensive testing infrastructure and simulation environments are coming soon. -Adding Car Support ------- - -comma.ai offers [bounties](http://comma.ai/bounties.html) for adding additional car support. - -CR-V Touring support came in through this program. Chevy Volt is close. Accord is close as well. - User Data / chffr Account / Crash Reporting ------ diff --git a/RELEASES.md b/RELEASES.md index fc3613c5ca..3e407c0698 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,12 @@ +Version 0.4.7 (2018-06-15) +========================== + * New model! + * GM Volt (and CT6 lateral) support! + * Honda Bosch lateral support! + * Improve actuator modeling to reduce lateral wobble + * Minor refactor of car abstraction layer + * Hack around orbd startup issue + Version 0.4.6 (2018-05-18) ========================== * NEOSv6 required! Will autoupdate diff --git a/cereal/car.capnp b/cereal/car.capnp index 19ab7120b5..a1444e5d78 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -58,6 +58,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { ipasOverride @33; debugAlert @34; steerTempUnavailableMute @35; + resumeRequired @36; } } @@ -295,6 +296,7 @@ struct CarParams { gm @4; hondaBosch @5; ford @6; + cadillac @7; } # things about the car in the manual @@ -332,6 +334,9 @@ struct CarParams { startAccel @35 :Float32; # Required acceleraton to overcome creep braking steerRateCost @40 :Float32; # Lateral MPC cost on steering rate steerControlType @46 :SteerControlType; + radarOffCan @47 :Bool; # True when radar objects aren't visible on CAN + + steerActuatorDelay @48 :Float32; # Steering wheel actuator delay in seconds enum SteerControlType { torque @0; diff --git a/common/fingerprints.py b/common/fingerprints.py index 4492f79040..51d78a34c7 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -1,111 +1,37 @@ -class HONDA: - CIVIC = "HONDA CIVIC 2016 TOURING" - ACURA_ILX = "ACURA ILX 2016 ACURAWATCH PLUS" - CRV = "HONDA CR-V 2016 TOURING" - ODYSSEY = "HONDA ODYSSEY 2018 EX-L" - ACURA_RDX = "ACURA RDX 2018 ACURAWATCH PLUS" - PILOT = "HONDA PILOT 2017 TOURING" - RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION" - - -class TOYOTA: - PRIUS = "TOYOTA PRIUS 2017" - RAV4H = "TOYOTA RAV4 2017 HYBRID" - RAV4 = "TOYOTA RAV4 2017" - COROLLA = "TOYOTA COROLLA 2017" - LEXUS_RXH = "LEXUS RX HYBRID 2017" - -class GM: - VOLT = "CHEVROLET VOLT PREMIER 2017" - CADILLAC_CT6 = "CADILLAC CT6 SUPERCRUISE 2018" - -class FORD: - FUSION = "FORD FUSION 2018" +import os +from common.basedir import BASEDIR + +def get_fingerprint_list(): + # read all the folders in selfdrive/car and return a dict where: + # - keys are all the car models for which we have a fingerprint + # - values are lists dicts of messages that constitute the unique + # CAN fingerprint of each car model and all its variants + fingerprints = {} + for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]: + try: + car_name = car_folder.split('/')[-1] + values = __import__('selfdrive.car.%s.values' % car_name, fromlist=['FINGERPRINTS']) + if hasattr(values, 'FINGERPRINTS'): + car_fingerprints = values.FINGERPRINTS + else: + continue + for f, v in car_fingerprints.iteritems(): + fingerprints[f] = v + except (ImportError, IOError): + pass + return fingerprints + + +_FINGERPRINTS = get_fingerprint_list() _DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes -_FINGERPRINTS = { - HONDA.ACURA_ILX: [{ - 1024L: 5, 513L: 6, 1027L: 5, 1029L: 8, 929L: 4, 1057L: 5, 777L: 8, 1034L: 5, 1036L: 8, 398L: 3, 399L: 7, 145L: 8, 660L: 8, 985L: 3, 923L: 2, 542L: 7, 773L: 7, 800L: 8, 432L: 7, 419L: 8, 420L: 8, 1030L: 5, 422L: 8, 808L: 8, 428L: 8, 304L: 8, 819L: 7, 821L: 5, 57L: 3, 316L: 8, 545L: 4, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 1039L: 8, 476L: 4, 892L: 8, 490L: 8, 1064L: 7, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 380L: 8, 1365L: 5, - # sent messages - 0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5, - }], - HONDA.ACURA_RDX: [{ - 57L: 3, 145L: 8, 229L: 4, 308L: 5, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 392L: 6, 398L: 3, 399L: 6, 404L: 4, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 892L: 8, 923L: 2, 929L: 4, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1034L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1365L: 5, 1424L: 5, 1729L: 1 - }], - HONDA.CIVIC: [{ - 1024L: 5, 513L: 6, 1027L: 5, 1029L: 8, 777L: 8, 1036L: 8, 1039L: 8, 1424L: 5, 401L: 8, 148L: 8, 662L: 4, 985L: 3, 795L: 8, 773L: 7, 800L: 8, 545L: 6, 420L: 8, 806L: 8, 808L: 8, 1322L: 5, 427L: 3, 428L: 8, 304L: 8, 432L: 7, 57L: 3, 450L: 8, 929L: 8, 330L: 8, 1302L: 8, 464L: 8, 1361L: 5, 1108L: 8, 597L: 8, 470L: 2, 344L: 8, 804L: 8, 399L: 7, 476L: 7, 1633L: 8, 487L: 4, 892L: 8, 490L: 8, 493L: 5, 884L: 8, 891L: 8, 380L: 8, 1365L: 5, - # sent messages - 0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8, - }], - HONDA.CRV: [{ - 57L: 3, 145L: 8, 316L: 8, 340L: 8, 342L: 6, 344L: 8, 380L: 8, 398L: 3, 399L: 6, 401L: 8, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 493L: 3, 507L: 1, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 661L: 4, 773L: 7, 777L: 8, 800L: 8, 804L: 8, 808L: 8, 882L: 2, 884L: 7, 888L: 8, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, - # sent messages - 0x194: 4, 0x1fa: 8, 0x30c: 8, 0x33d: 5, - }], - HONDA.ODYSSEY: [{ - 57L: 3, 148L: 8, 228L: 5, 229L: 4, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1615L: 8, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5 - }, - # Odyssey Elite - { - 57L: 3, 148L: 8, 228L: 5, 229L: 4, 304L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 440L: 8, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 507L: 1, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5 - }], - # Includes 2017 Touring and 2016 EX-L messaging. - HONDA.PILOT: [{ - 57L: 3, 145L: 8, 228L: 5, 229L: 4, 308L: 5, 316L: 8, 334L: 8, 339L: 7, 342L: 6, 344L: 8, 379L: 8, 380L: 8, 392L: 6, 399L: 7, 419L: 8, 420L: 8, 422L: 8, 425L: 8, 426L: 8, 427L: 3, 432L: 7, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 507L: 1, 538L: 3, 542L: 7, 545L: 5, 546L: 3, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 837L: 5, 856L: 7, 871L: 8, 882L: 2, 884L: 7, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1027L: 5, 1029L: 8, 1036L: 8, 1039L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1108L: 8, 1125L: 8, 1296L: 8, 1424L: 5, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1616L: 5, 1618L: 5, 1668L: 5 - }], - HONDA.RIDGELINE: [{ - 57L: 3, 145L: 8, 228L: 5, 229L: 4, 308L: 5, 316L: 8, 339L: 7, 342L: 6, 344L: 8, 380L: 8, 392L: 6, 399L: 7, 419L: 8, 420L: 8, 422L: 8, 425L: 8, 426L: 8, 427L: 3, 432L: 7, 464L: 8, 471L: 3, 476L: 4, 490L: 8, 506L: 8, 545L: 5, 546L: 3, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 871L: 8, 882L: 2, 884L: 7, 892L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1027L: 5, 1029L: 8, 1036L: 8, 1039L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, 1613L: 5, 1616L: 5, 1618L: 5, 1668L: 5, 2015L: 3 - }], - TOYOTA.RAV4: [{ - 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 - }], - TOYOTA.RAV4H: [{ - 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 296L: 8, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 4, 581L: 5, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 713L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 3, 955L: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1184L: 8, 1185L: 8, 1186L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1197L: 8, 1198L: 8, 1199L: 8, 1212L: 8, 1227L: 8, 1228L: 8, 1232L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 - }], - TOYOTA.PRIUS: [{ - 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 - }, - # Prius Prime - { - 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 824L: 2, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2,898L: 8, 900L: 6, 902L: 6, 905L: 8, 913L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 974L: 8, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1076L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1164L: 8, 1165L: 8, 1166L: 8, 1167L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 - }, - # Taiwanese Prius Prime - { - 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 824L: 2, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 845L: 5, 863L: 8, 869L: 7, 870L: 7, 871L: 2,898L: 8, 900L: 6, 902L: 6, 905L: 8, 913L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 974L: 8, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1076L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1164L: 8, 1165L: 8, 1166L: 8, 1167L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1264L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 - }], - TOYOTA.COROLLA: [{ - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 - }, - # Corolla LE 2017 - { - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8 - }], - TOYOTA.LEXUS_RXH: [{ - 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8 - }], - GM.VOLT: [{ - 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 - }], - GM.CADILLAC_CT6: [{ - 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 336: 1, 338: 6, 340: 6, 352: 5, 354: 5, 356: 8, 368: 3, 372: 5, 381: 8, 386: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 458: 5, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 800: 6, 801: 8, 804: 3, 810: 8, 832: 8, 833: 8, 834: 8, 835: 6, 836: 5, 837: 8, 838: 8, 839: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 884: 8, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 1, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1934: 7, 2016: 8, 2024: 8 - }], - FORD.FUSION: [{ - 71: 8, 74: 8, 75: 8, 76: 8, 90: 8, 92: 8, 93: 8, 118: 8, 119: 8, 120: 8, 125: 8, 129: 8, 130: 8, 131: 8, 132: 8, 133: 8, 145: 8, 146: 8, 357: 8, 359: 8, 360: 8, 361: 8, 376: 8, 390: 8, 391: 8, 392: 8, 394: 8, 512: 8, 514: 8, 516: 8, 531: 8, 532: 8, 534: 8, 535: 8, 560: 8, 578: 8, 604: 8, 613: 8, 673: 8, 827: 8, 848: 8, 934: 8, 935: 8, 936: 8, 947: 8, 963: 8, 970: 8, 972: 8, 973: 8, 984: 8, 992: 8, 994: 8, 997: 8, 998: 8, 1003: 8, 1034: 8, 1045: 8, 1046: 8, 1053: 8, 1054: 8, 1058: 8, 1059: 8, 1068: 8, 1072: 8, 1073: 8, 1082: 8, 1107: 8, 1108: 8, 1109: 8, 1110: 8, 1200: 8, 1427: 8, 1430: 8, 1438: 8, 1459: 8 - }], -} - -# support additional internal only fingerprints -try: - from common.fingerprints_internal import add_additional_fingerprints - add_additional_fingerprints(_FINGERPRINTS) -except ImportError: - pass - - def is_valid_for_fingerprint(msg, car_fingerprint): adr = msg.address - return msg.src != 0 or (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) + bus = msg.src + # ignore addresses that are more than 11 bits + return (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) or \ + bus != 0 or adr >= 0x800 def eliminate_incompatible_cars(msg, candidate_cars): @@ -119,6 +45,7 @@ def eliminate_incompatible_cars(msg, candidate_cars): A list containing the subset of candidate_cars that could have sent msg. """ compatible_cars = [] + for car_name in candidate_cars: car_fingerprints = _FINGERPRINTS[car_name] diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 771ea7d99f..66eef809a1 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -38,6 +38,7 @@ #define SAFETY_GM 3 #define SAFETY_HONDA_BOSCH 4 #define SAFETY_FORD 5 +#define SAFETY_CADILLAC 6 #define SAFETY_TOYOTA_NOLIMITS 0x1336 #define SAFETY_ALLOUTPUT 0x1337 @@ -109,6 +110,9 @@ void *safety_setter_thread(void *s) { case (int)cereal::CarParams::SafetyModels::FORD: safety_setting = SAFETY_FORD; break; + case (int)cereal::CarParams::SafetyModels::CADILLAC: + safety_setting = SAFETY_CADILLAC; + break; default: LOGE("unknown safety model: %d", safety_model); } diff --git a/selfdrive/can/common.h b/selfdrive/can/common.h index 2741e39715..accb593ff3 100644 --- a/selfdrive/can/common.h +++ b/selfdrive/can/common.h @@ -48,6 +48,7 @@ struct Signal { int b1, b2, bo; bool is_signed; double factor, offset; + bool is_little_endian; SignalType type; }; diff --git a/selfdrive/can/dbc_template.cc b/selfdrive/can/dbc_template.cc index 7d15691545..3615e05297 100644 --- a/selfdrive/can/dbc_template.cc +++ b/selfdrive/can/dbc_template.cc @@ -8,7 +8,11 @@ namespace { const Signal sigs_{{address}}[] = { {% for sig in sigs %} { - {% set b1 = (sig.start_bit//8)*8 + (-sig.start_bit-1) % 8 %} + {% if sig.is_little_endian %} + {% set b1 = sig.start_bit %} + {% else %} + {% set b1 = (sig.start_bit//8)*8 + (-sig.start_bit-1) % 8 %} + {% endif %} .name = "{{sig.name}}", .b1 = {{b1}}, .b2 = {{sig.size}}, @@ -16,6 +20,7 @@ const Signal sigs_{{address}}[] = { .is_signed = {{"true" if sig.is_signed else "false"}}, .factor = {{sig.factor}}, .offset = {{sig.offset}}, + .is_little_endian = {{"true" if sig.is_little_endian else "false"}}, {% if checksum_type == "honda" and sig.name == "CHECKSUM" %} .type = SignalType::HONDA_CHECKSUM, {% elif checksum_type == "honda" and sig.name == "COUNTER" %} diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 7f23c159eb..9246ae5b7e 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -64,6 +64,17 @@ uint64_t read_u64_be(const uint8_t* v) { | (uint64_t)v[7]); } +uint64_t read_u64_le(const uint8_t* v) { + return ((uint64_t)v[0] + | ((uint64_t)v[1] << 8) + | ((uint64_t)v[2] << 16) + | ((uint64_t)v[3] << 24) + | ((uint64_t)v[4] << 32) + | ((uint64_t)v[5] << 40) + | ((uint64_t)v[6] << 48) + | ((uint64_t)v[7] << 56)); +} + struct MessageState { uint32_t address; @@ -82,8 +93,14 @@ struct MessageState { bool parse(uint64_t sec, uint16_t ts_, uint64_t dat) { for (int i=0; i < parse_sigs.size(); i++) { auto& sig = parse_sigs[i]; + int64_t tmp; + + if (sig.is_little_endian){ + tmp = (dat >> sig.b1) & ((1ULL << sig.b2)-1); + } else { + tmp = (dat >> sig.bo) & ((1ULL << sig.b2)-1); + } - int64_t tmp = (dat >> sig.bo) & ((1ULL << sig.b2)-1); if (sig.is_signed) { tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed } @@ -220,6 +237,7 @@ class CANParser { void UpdateCans(uint64_t sec, const capnp::List::Reader& cans) { int msg_count = cans.size(); + uint64_t p; DEBUG("got %d messages\n", msg_count); @@ -240,7 +258,13 @@ class CANParser { uint8_t dat[8] = {0}; memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size()); - uint64_t p = read_u64_be(dat); + // Assumes all signals in the message are of the same type (little or big endian) + auto& sig = message_states[cmsg.getAddress()].parse_sigs[0]; + if (sig.is_little_endian) { + p = read_u64_le(dat); + } else { + p = read_u64_be(dat); + } DEBUG(" proc %X: %llx\n", cmsg.getAddress(), p); diff --git a/selfdrive/car/honda/old_can_parser.py b/selfdrive/can/plant_can_parser.py similarity index 98% rename from selfdrive/car/honda/old_can_parser.py rename to selfdrive/can/plant_can_parser.py index da47ecd703..44939747ce 100644 --- a/selfdrive/car/honda/old_can_parser.py +++ b/selfdrive/can/plant_can_parser.py @@ -1,3 +1,4 @@ +### old can parser just used by plant.py for regression tests import os import opendbc from collections import defaultdict diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 5029ba2c53..80e79f0349 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,87 +1,4 @@ -import os +# functions common among cars -from common.realtime import sec_since_boot -from common.fingerprints import eliminate_incompatible_cars, all_known_cars - -from selfdrive.swaglog import cloudlog -import selfdrive.messaging as messaging -from common.fingerprints import HONDA, TOYOTA, GM, FORD - -def load_interfaces(x): - ret = {} - for interface in x: - try: - imp = __import__('selfdrive.car.%s.interface' % interface, fromlist=['CarInterface']).CarInterface - except ImportError: - imp = None - for car in x[interface]: - ret[car] = imp - return ret - -# imports from directory selfdrive/car// -interfaces = load_interfaces({ - 'honda': [HONDA.CIVIC, HONDA.ACURA_ILX, HONDA.CRV, HONDA.ODYSSEY, HONDA.ACURA_RDX, HONDA.PILOT, HONDA.RIDGELINE], - 'toyota': [TOYOTA.PRIUS, TOYOTA.RAV4, TOYOTA.RAV4H, TOYOTA.COROLLA, TOYOTA.LEXUS_RXH], - 'gm': [GM.VOLT, GM.CADILLAC_CT6], - 'ford': [FORD.FUSION], - 'simulator2': ['simulator2'], - 'mock': ['mock']}) - -# **** for use live only **** -def fingerprint(logcan, timeout): - if os.getenv("SIMULATOR2") is not None: - return ("simulator2", None) - - finger_st = sec_since_boot() - - cloudlog.warning("waiting for fingerprint...") - candidate_cars = all_known_cars() - finger = {} - st = None - while 1: - for a in messaging.drain_sock(logcan, wait_for_one=True): - if st is None: - st = sec_since_boot() - for can in a.can: - if can.src == 0: - finger[can.address] = len(can.dat) - candidate_cars = eliminate_incompatible_cars(can, candidate_cars) - - ts = sec_since_boot() - # if we only have one car choice and the time_fingerprint since we got our first - # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not - # broadcast immediately - if len(candidate_cars) == 1 and st is not None: - # TODO: better way to decide to wait more if Toyota - time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1 - if (ts-st) > time_fingerprint: - break - - # bail if no cars left or we've been waiting too long - elif len(candidate_cars) == 0 or (timeout and ts-finger_st > timeout): - return None, finger - - cloudlog.warning("fingerprinted %s", candidate_cars[0]) - return (candidate_cars[0], finger) - - -def get_car(logcan, sendcan=None, passive=True): - # TODO: timeout only useful for replays so controlsd can start before unlogger - timeout = 1. if passive else None - candidate, fingerprints = fingerprint(logcan, timeout) - - if candidate is None: - cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) - if passive: - candidate = "mock" - else: - return None, None - - interface_cls = interfaces[candidate] - if interface_cls is None: - cloudlog.warning("car matched %s, but interface wasn't available" % candidate) - return None, None - - params = interface_cls.get_params(candidate, fingerprints) - - return interface_cls(params, sendcan), params +def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None): + return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc} diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py new file mode 100644 index 0000000000..0255137ab0 --- /dev/null +++ b/selfdrive/car/car_helpers.py @@ -0,0 +1,101 @@ +import os +from common.basedir import BASEDIR +from common.realtime import sec_since_boot +from common.fingerprints import eliminate_incompatible_cars, all_known_cars +from selfdrive.swaglog import cloudlog +import selfdrive.messaging as messaging + +def load_interfaces(x): + ret = {} + for interface in x: + try: + imp = __import__('selfdrive.car.%s.interface' % interface, fromlist=['CarInterface']).CarInterface + except ImportError: + imp = None + for car in x[interface]: + ret[car] = imp + return ret + + +def _get_interface_names(): + # read all the folders in selfdrive/car and return a dict where: + # - keys are all the car names that which we have an interface for + # - values are lists of spefic car models for a given car + interface_names = {} + for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]: + try: + car_name = car_folder.split('/')[-1] + model_names = __import__('selfdrive.car.%s.values' % car_name, fromlist=['CAR']).CAR + model_names = [getattr(model_names, c) for c in model_names.__dict__.keys() if not c.startswith("__")] + interface_names[car_name] = model_names + except (ImportError, IOError): + pass + + return interface_names + + +# imports from directory selfdrive/car// +interfaces = load_interfaces(_get_interface_names()) + + +# **** for use live only **** +def fingerprint(logcan, timeout): + if os.getenv("SIMULATOR2") is not None: + return ("simulator2", None) + elif os.getenv("SIMULATOR") is not None: + return ("simulator", None) + + cloudlog.warning("waiting for fingerprint...") + candidate_cars = all_known_cars() + finger = {} + st = None + while 1: + for a in messaging.drain_sock(logcan, wait_for_one=True): + for can in a.can: + # ignore everything not on bus 0 and with more than 11 bits, + # which are ussually sporadic and hard to include in fingerprints + if can.src == 0 and can.address < 0x800: + finger[can.address] = len(can.dat) + candidate_cars = eliminate_incompatible_cars(can, candidate_cars) + + if st is None: + st = sec_since_boot() # start time + st_passive = sec_since_boot() # only relevant when passive + ts = sec_since_boot() + # if we only have one car choice and the time_fingerprint since we got our first + # message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not + # broadcast immediately + if len(candidate_cars) == 1 and st is not None: + # TODO: better way to decide to wait more if Toyota + time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1 + if (ts-st) > time_fingerprint: + break + + # bail if no cars left or we've been waiting too long + elif len(candidate_cars) == 0 or (timeout and (ts - st_passive) > timeout): + return None, finger + + cloudlog.warning("fingerprinted %s", candidate_cars[0]) + return (candidate_cars[0], finger) + + +def get_car(logcan, sendcan=None, passive=True): + # TODO: timeout only useful for replays so controlsd can start before unlogger + timeout = 1. if passive else None + candidate, fingerprints = fingerprint(logcan, timeout) + + if candidate is None: + cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) + if passive: + candidate = "mock" + else: + return None, None + + interface_cls = interfaces[candidate] + if interface_cls is None: + cloudlog.warning("car matched %s, but interface wasn't available" % candidate) + return None, None + + params = interface_cls.get_params(candidate, fingerprints) + + return interface_cls(params, sendcan), params diff --git a/selfdrive/car/ford/__init__.py b/selfdrive/car/ford/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py new file mode 100644 index 0000000000..f85e2155a4 --- /dev/null +++ b/selfdrive/car/ford/carstate.py @@ -0,0 +1,105 @@ +from selfdrive.can.parser import CANParser +from selfdrive.config import Conversions as CV +from selfdrive.car.ford.values import DBC +from common.kalman.simple_kalman import KF1D +import numpy as np + +WHEEL_RADIUS = 0.33 + +def parse_gear_shifter(can_gear, car_fingerprint): + # TODO: Use values from DBC to parse this field + if can_gear == 0x0: + return "park" + elif can_gear == 0x1: + return "reverse" + elif can_gear == 0x2: + return "neutral" + elif can_gear == 0x3: + return "drive" + elif can_gear == 0x4: + return "brake" + return "unknown" + + +def get_can_parser(CP): + + signals = [ + # sig_name, sig_address, default + ("WhlRr_W_Meas", "WheelSpeed_CG1", 0.), + ("WhlRl_W_Meas", "WheelSpeed_CG1", 0.), + ("WhlFr_W_Meas", "WheelSpeed_CG1", 0.), + ("WhlFl_W_Meas", "WheelSpeed_CG1", 0.), + ("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1", 0.), + ("Cruise_State", "Cruise_Status", 0.), + ("Set_Speed", "Cruise_Status", 0.), + ("LaActAvail_D_Actl", "Lane_Keep_Assist_Status", 0), + ("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status", 0), + ("LaActDeny_B_Actl", "Lane_Keep_Assist_Status", 0), + ("ApedPosScal_Pc_Actl", "EngineData_14", 0.), + ("Dist_Incr", "Steering_Buttons", 0.), + ("Brake_Drv_Appl", "Cruise_Status", 0.), + ("Brake_Lights", "BCM_to_HS_Body", 0.), + ] + + checks = [ + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + + +class CarState(object): + def __init__(self, CP): + + self.CP = CP + self.left_blinker_on = 0 + self.right_blinker_on = 0 + + # initialize can parser + self.car_fingerprint = CP.carFingerprint + + # vEgo kalman filter + dt = 0.01 + # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) + # R = 1e3 + self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]), + A=np.matrix([[1.0, dt], [0.0, 1.0]]), + C=np.matrix([1.0, 0.0]), + K=np.matrix([[0.12287673], [0.29666309]])) + self.v_ego = 0.0 + + def update(self, cp): + # copy can_valid + self.can_valid = cp.can_valid + + # update prevs, update must run once per loop + self.prev_left_blinker_on = self.left_blinker_on + self.prev_right_blinker_on = self.right_blinker_on + + # calc best v_ego estimate, by averaging two opposite corners + self.v_wheel_fl = cp.vl["WheelSpeed_CG1"]['WhlRr_W_Meas'] * WHEEL_RADIUS + self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS + self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS + self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS + self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. + + # Kalman filter + if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed + self.v_ego_x = np.matrix([[self.v_wheel], [0.0]]) + + self.v_ego_raw = self.v_wheel + v_ego_x = self.v_ego_kf.update(self.v_wheel) + self.v_ego = float(v_ego_x[0]) + self.a_ego = float(v_ego_x[1]) + self.standstill = not self.v_wheel > 0.001 + + self.angle_steers = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns'] + self.v_cruise_pcm = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS + self.pcm_acc_status = cp.vl["Cruise_Status"]['Cruise_State'] + self.main_on = cp.vl["Cruise_Status"]['Cruise_State'] != 0 + self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl'] + self.steer_override = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl'] + self.steer_error = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] + self.user_gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl'] + self.brake_pressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"]) + self.brake_lights = bool(cp.vl["BCM_to_HS_Body"]["Brake_Lights"]) + self.generic_toggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"]) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py new file mode 100755 index 0000000000..663d521874 --- /dev/null +++ b/selfdrive/car/ford/interface.py @@ -0,0 +1,217 @@ +#!/usr/bin/env python +from common.realtime import sec_since_boot +from cereal import car +from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.car.ford.carstate import CarState, get_can_parser +from selfdrive.car.ford.fordcan import MAX_ANGLE + +try: + from selfdrive.car.ford.carcontroller import CarController +except ImportError: + CarController = None + + +class CarInterface(object): + def __init__(self, CP, sendcan=None): + self.CP = CP + self.VM = VehicleModel(CP) + + self.frame = 0 + self.can_invalid_count = 0 + self.gas_pressed_prev = False + self.brake_pressed_prev = False + self.cruise_enabled_prev = False + + # *** init the major players *** + self.CS = CarState(CP) + + self.cp = get_can_parser(CP) + + # sending if read only is False + if sendcan is not None: + self.sendcan = sendcan + self.CC = CarController(self.cp.dbc_name, CP.enableCamera, self.VM) + + @staticmethod + def compute_gb(accel, speed): + return float(accel) / 3.0 + + @staticmethod + def calc_accel_override(a_ego, a_target, v_ego, v_target): + return 1.0 + + @staticmethod + def get_params(candidate, fingerprint): + + # kg of standard extra cargo to count for drive, gas, etc... + std_cargo = 136 + + ret = car.CarParams.new_message() + + ret.carName = "ford" + ret.carFingerprint = candidate + + ret.safetyModel = car.CarParams.SafetyModels.ford + + # pedal + ret.enableCruise = True + + # FIXME: hardcoding honda civic 2016 touring params so they can be used to + # scale unknown params for other cars + mass_civic = 2923./2.205 + std_cargo + wheelbase_civic = 2.70 + centerToFront_civic = wheelbase_civic * 0.4 + centerToRear_civic = wheelbase_civic - centerToFront_civic + rotationalInertia_civic = 2500 + tireStiffnessFront_civic = 85400 + tireStiffnessRear_civic = 90000 + + ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] + ret.wheelbase = 2.85 + ret.steerRatio = 14.8 + ret.mass = 3045./2.205 + std_cargo + ret.steerKpV, ret.steerKiV = [[0.01], [0.005]] # TODO: tune this + ret.steerKf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF + ret.steerActuatorDelay = 0.1 # Default delay, not measured yet + ret.steerRateCost = 0.5 + + f = 1.2 + tireStiffnessFront_civic *= f + tireStiffnessRear_civic *= f + + ret.centerToFront = ret.wheelbase * 0.44 + + ret.longPidDeadzoneBP = [0., 9.] + ret.longPidDeadzoneV = [0., .15] + + # min speed to enable ACC. if car can do stop and go, then set enabling speed + # to a negative value, so it won't matter. + ret.minEnableSpeed = -1. + + centerToRear = ret.wheelbase - ret.centerToFront + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = rotationalInertia_civic * \ + ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront = tireStiffnessFront_civic * \ + ret.mass / mass_civic * \ + (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) + ret.tireStiffnessRear = tireStiffnessRear_civic * \ + ret.mass / mass_civic * \ + (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + + # no rear steering, at least on the listed cars above + ret.steerRatioRear = 0. + ret.steerControlType = car.CarParams.SteerControlType.angle + + # steer, gas, brake limitations VS speed + ret.steerMaxBP = [0.] # breakpoints at 1 and 40 kph + ret.steerMaxV = [1.0] # 2/3rd torque allowed above 45 kph + ret.gasMaxBP = [0.] + ret.gasMaxV = [0.5] + ret.brakeMaxBP = [5., 20.] + ret.brakeMaxV = [1., 0.8] + + ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint) + print "ECU Camera Simulated: ", ret.enableCamera + + ret.steerLimitAlert = False + ret.stoppingControl = False + ret.startAccel = 0.0 + + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [3.6, 2.4, 1.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.54, 0.36] + + return ret + + # returns a car.CarState + def update(self, c): + # ******************* do can recv ******************* + canMonoTimes = [] + + self.cp.update(int(sec_since_boot() * 1e9), False) + + self.CS.update(self.cp) + + # create message + ret = car.CarState.new_message() + + # speeds + ret.vEgo = self.CS.v_ego + ret.vEgoRaw = self.CS.v_ego_raw + ret.standstill = self.CS.standstill + ret.wheelSpeeds.fl = self.CS.v_wheel_fl + ret.wheelSpeeds.fr = self.CS.v_wheel_fr + ret.wheelSpeeds.rl = self.CS.v_wheel_rl + ret.wheelSpeeds.rr = self.CS.v_wheel_rr + + # steering wheel + ret.steeringAngle = self.CS.angle_steers + ret.steeringPressed = self.CS.steer_override + + # gas pedal + ret.gas = self.CS.user_gas / 100. + ret.gasPressed = self.CS.user_gas > 0.0001 + ret.brakePressed = self.CS.brake_pressed + ret.brakeLights = self.CS.brake_lights + + ret.cruiseState.enabled = not (self.CS.pcm_acc_status in [0, 3]) + ret.cruiseState.speed = self.CS.v_cruise_pcm + ret.cruiseState.available = self.CS.pcm_acc_status != 0 + + ret.genericToggle = self.CS.generic_toggle + + # events + events = [] + if not self.CS.can_valid: + self.can_invalid_count += 1 + if self.can_invalid_count >= 5: + events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + else: + self.can_invalid_count = 0 + + if self.CS.steer_error: + events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) + + # enable request in prius is simple, as we activate when Toyota is active (rising edge) + if ret.cruiseState.enabled and not self.cruise_enabled_prev: + events.append(create_event('pcmEnable', [ET.ENABLE])) + elif not ret.cruiseState.enabled: + events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + + # disable on pedals rising edge or when brake is pressed and speed isn't zero + if (ret.gasPressed and not self.gas_pressed_prev) or \ + (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + + if ret.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + + if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13.* CV.MPH_TO_MS and ret.cruiseState.enabled: + events.append(create_event('steerTempUnavailableMute', [ET.WARNING])) + + ret.events = events + ret.canMonoTimes = canMonoTimes + + self.gas_pressed_prev = ret.gasPressed + self.brake_pressed_prev = ret.brakePressed + self.cruise_enabled_prev = ret.cruiseState.enabled + + return ret.as_reader() + + # pass in a car.CarControl + # to be called @ 100hz + def apply(self, c): + + self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, + c.hudControl.visualAlert, c.cruiseControl.cancel) + + self.frame += 1 + return False diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py new file mode 100755 index 0000000000..2acea9d708 --- /dev/null +++ b/selfdrive/car/ford/radar_interface.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python +import os +import numpy as np +from selfdrive.can.parser import CANParser +from cereal import car +from common.realtime import sec_since_boot +import zmq +from selfdrive.services import service_list +import selfdrive.messaging as messaging + + +RADAR_MSGS = range(0x500, 0x540) + +def _create_radard_can_parser(): + dbc_f = 'ford_fusion_2018_adas.dbc' + msg_n = len(RADAR_MSGS) + signals = zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, + RADAR_MSGS * 3, + [0] * msg_n + [0] * msg_n + [0] * msg_n) + checks = zip(RADAR_MSGS, [20]*msg_n) + + return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) + +class RadarInterface(object): + def __init__(self, CP): + # radar + self.pts = {} + self.validCnt = {key: 0 for key in RADAR_MSGS} + self.track_id = 0 + + self.delay = 0.0 # Delay of radar + + # Nidec + self.rcp = _create_radard_can_parser() + + context = zmq.Context() + self.logcan = messaging.sub_sock(context, service_list['can'].port) + + def update(self): + canMonoTimes = [] + + updated_messages = set() + while 1: + tm = int(sec_since_boot() * 1e9) + updated_messages.update(self.rcp.update(tm, True)) + # TODO: do not hardcode last msg + if 0x53f in updated_messages: + break + + ret = car.RadarState.new_message() + errors = [] + if not self.rcp.can_valid: + errors.append("commIssue") + ret.errors = errors + ret.canMonoTimes = canMonoTimes + + for ii in updated_messages: + cpt = self.rcp.vl[ii] + + if cpt['X_Rel'] > 0.00001: + self.validCnt[ii] = 0 # reset counter + + if cpt['X_Rel'] > 0.00001: + self.validCnt[ii] += 1 + else: + self.validCnt[ii] = max(self.validCnt[ii] -1, 0) + #print ii, self.validCnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle'] + + # radar point only valid if there have been enough valid measurements + if self.validCnt[ii] > 0: + if ii not in self.pts: + self.pts[ii] = car.RadarState.RadarPoint.new_message() + self.pts[ii].trackId = self.track_id + self.track_id += 1 + self.pts[ii].dRel = cpt['X_Rel'] # from front of car + self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * np.pi / 180. # in car frame's y axis, left is positive + self.pts[ii].vRel = cpt['V_Rel'] + self.pts[ii].aRel = float('nan') + self.pts[ii].yvRel = float('nan') + self.pts[ii].measured = True + else: + if ii in self.pts: + del self.pts[ii] + + ret.points = self.pts.values() + return ret + +if __name__ == "__main__": + RI = RadarInterface(None) + while 1: + ret = RI.update() + print(chr(27) + "[2J") + print ret diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py new file mode 100644 index 0000000000..b8890107fc --- /dev/null +++ b/selfdrive/car/ford/values.py @@ -0,0 +1,14 @@ +from selfdrive.car import dbc_dict + +class CAR: + FUSION = "FORD FUSION 2018" + +FINGERPRINTS = { + CAR.FUSION: [{ + 71: 8, 74: 8, 75: 8, 76: 8, 90: 8, 92: 8, 93: 8, 118: 8, 119: 8, 120: 8, 125: 8, 129: 8, 130: 8, 131: 8, 132: 8, 133: 8, 145: 8, 146: 8, 357: 8, 359: 8, 360: 8, 361: 8, 376: 8, 390: 8, 391: 8, 392: 8, 394: 8, 512: 8, 514: 8, 516: 8, 531: 8, 532: 8, 534: 8, 535: 8, 560: 8, 578: 8, 604: 8, 613: 8, 673: 8, 827: 8, 848: 8, 934: 8, 935: 8, 936: 8, 947: 8, 963: 8, 970: 8, 972: 8, 973: 8, 984: 8, 992: 8, 994: 8, 997: 8, 998: 8, 1003: 8, 1034: 8, 1045: 8, 1046: 8, 1053: 8, 1054: 8, 1058: 8, 1059: 8, 1068: 8, 1072: 8, 1073: 8, 1082: 8, 1107: 8, 1108: 8, 1109: 8, 1110: 8, 1200: 8, 1427: 8, 1430: 8, 1438: 8, 1459: 8 + }], +} + +DBC = { + CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'), +} diff --git a/selfdrive/car/gm/__init__.py b/selfdrive/car/gm/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py new file mode 100644 index 0000000000..d3c8ee1967 --- /dev/null +++ b/selfdrive/car/gm/carcontroller.py @@ -0,0 +1,192 @@ +from common.numpy_fast import clip, interp +from common.realtime import sec_since_boot +from selfdrive.config import Conversions as CV +from selfdrive.boardd.boardd import can_list_to_can_capnp +from selfdrive.car.gm import gmcan +from selfdrive.car.gm.values import CAR, DBC +from selfdrive.can.packer import CANPacker + + +class CarControllerParams(): + def __init__(self, car_fingerprint): + if car_fingerprint == CAR.VOLT: + self.STEER_MAX = 255 + self.STEER_STEP = 2 # how often we update the steer cmd + self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s) + self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero + elif car_fingerprint == CAR.CADILLAC_CT6: + self.STEER_MAX = 150 + self.STEER_STEP = 1 # how often we update the steer cmd + self.STEER_DELTA_UP = 2 # 0.75s time to peak torque + self.STEER_DELTA_DOWN = 5 # 0.3s from peak torque to zero + + self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting + self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily + self.STEER_DRIVER_FACTOR = 100 # from dbc + self.NEAR_STOP_BRAKE_PHASE = 0.5 # m/s, more aggressive braking near full stop + + self.ADAS_KEEPALIVE_STEP = 10 + # pedal lookups, only for Volt + MAX_GAS = 3072 # Only a safety limit + ZERO_GAS = 2048 + MAX_BRAKE = 350 # Should be around 3.5m/s^2, including regen + self.MAX_ACC_REGEN = 1404 # ACC Regen braking is slightly less powerful than max regen paddle + self.GAS_LOOKUP_BP = [-0.25, 0., 0.5] + self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] + self.BRAKE_LOOKUP_BP = [-1., -0.25] + self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0] + + +def actuator_hystereses(final_pedal, pedal_steady): + # hyst params... TODO: move these to VehicleParams + pedal_hyst_gap = 0.01 # don't change pedal command for small oscilalitons within this value + + # for small pedal oscillations within pedal_hyst_gap, don't change the pedal command + if final_pedal == 0.: + pedal_steady = 0. + elif final_pedal > pedal_steady + pedal_hyst_gap: + pedal_steady = final_pedal - pedal_hyst_gap + elif final_pedal < pedal_steady - pedal_hyst_gap: + pedal_steady = final_pedal + pedal_hyst_gap + final_pedal = pedal_steady + + return final_pedal, pedal_steady + + +class CarController(object): + def __init__(self, canbus, car_fingerprint): + self.pedal_steady = 0. + self.start_time = sec_since_boot() + self.chime = 0 + self.lkas_active = False + self.inhibit_steer_for = 0 + self.steer_idx = 0 + self.apply_steer_last = 0 + self.car_fingerprint = car_fingerprint + + # Setup detection helper. Routes commands to + # an appropriate CAN bus number. + self.canbus = canbus + self.params = CarControllerParams(car_fingerprint) + + self.packer_pt = CANPacker(DBC[car_fingerprint]['pt']) + self.packer_ch = CANPacker(DBC[car_fingerprint]['chassis']) + + def update(self, sendcan, enabled, CS, frame, actuators, \ + hud_v_cruise, hud_show_lanes, hud_show_car, chime, chime_cnt): + """ Controls thread """ + + P = self.params + + # Send CAN commands. + can_sends = [] + canbus = self.canbus + + ### STEER ### + + if (frame % P.STEER_STEP) == 0: + final_steer = actuators.steer if enabled else 0. + apply_steer = final_steer * P.STEER_MAX + + # limits due to driver torque + driver_max_torque = P.STEER_MAX + (P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver * P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER + driver_min_torque = -P.STEER_MAX + (-P.STEER_DRIVER_ALLOWANCE + CS.steer_torque_driver * P.STEER_DRIVER_FACTOR) * P.STEER_DRIVER_MULTIPLIER + max_steer_allowed = max(min(P.STEER_MAX, driver_max_torque), 0) + min_steer_allowed = min(max(-P.STEER_MAX, driver_min_torque), 0) + apply_steer = clip(apply_steer, min_steer_allowed, max_steer_allowed) + + # slow rate if steer torque increases in magnitude + if self.apply_steer_last > 0: + apply_steer = clip(apply_steer, max(self.apply_steer_last - P.STEER_DELTA_DOWN, -P.STEER_DELTA_UP), + self.apply_steer_last + P.STEER_DELTA_UP) + else: + apply_steer = clip(apply_steer, self.apply_steer_last - P.STEER_DELTA_UP, + min(self.apply_steer_last + P.STEER_DELTA_DOWN, P.STEER_DELTA_UP)) + + lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3. + + if not lkas_enabled: + apply_steer = 0 + + apply_steer = int(round(apply_steer)) + self.apply_steer_last = apply_steer + idx = (frame / P.STEER_STEP) % 4 + + if self.car_fingerprint == CAR.VOLT: + can_sends.append(gmcan.create_steering_control(self.packer_pt, + canbus.powertrain, apply_steer, idx, lkas_enabled)) + if self.car_fingerprint == CAR.CADILLAC_CT6: + can_sends += gmcan.create_steering_control_ct6(self.packer_pt, + canbus, apply_steer, CS.v_ego, idx, lkas_enabled) + + ### GAS/BRAKE ### + + if self.car_fingerprint == CAR.VOLT: + # no output if not enabled, but keep sending keepalive messages + # threat pedals as one + final_pedal = actuators.gas - actuators.brake + + # *** apply pedal hysteresis *** + final_brake, self.brake_steady = actuator_hystereses( + final_pedal, self.pedal_steady) + + if not enabled: + apply_gas = P.MAX_ACC_REGEN # TODO: do we really need to send max regen when not enabled? + apply_brake = 0 + else: + apply_gas = int(round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) + apply_brake = int(round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) + + # Gas/regen and brakes - all at 25Hz + if (frame % 4) == 0: + idx = (frame / 4) % 4 + + at_full_stop = enabled and CS.standstill + near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) + can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop)) + + at_full_stop = enabled and CS.standstill + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop)) + + # Send dashboard UI commands (ACC status), 25hz + if (frame % 4) == 0: + can_sends.append(gmcan.create_acc_dashboard_command(canbus.powertrain, enabled, hud_v_cruise / CV.MS_TO_KPH, hud_show_car)) + + # Radar needs to know current speed and yaw rate (50hz), + # and that ADAS is alive (10hz) + time_and_headlights_step = 10 + tt = sec_since_boot() + + if frame % time_and_headlights_step == 0: + idx = (frame / time_and_headlights_step) % 4 + can_sends.append(gmcan.create_adas_time_status(canbus.obstacle, int((tt - self.start_time) * 60), idx)) + can_sends.append(gmcan.create_adas_headlights_status(canbus.obstacle)) + + speed_and_accelerometer_step = 2 + if frame % speed_and_accelerometer_step == 0: + idx = (frame / speed_and_accelerometer_step) % 4 + can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx)) + can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx)) + + # Send ADAS keepalive, 10hz + if frame % P.ADAS_KEEPALIVE_STEP == 0: + can_sends += gmcan.create_adas_keepalive(canbus.powertrain) + + # Send chimes + if self.chime != chime: + duration = 0x3c + + # There is no 'repeat forever' chime command + # TODO: Manage periodic re-issuing of chime command + # and chime cancellation + if chime_cnt == -1: + chime_cnt = 10 + + if chime != 0: + can_sends.append(gmcan.create_chime_command(canbus.sw_gmlan, chime, duration, chime_cnt)) + + # If canceling a repeated chime, cancel command must be + # issued for the same chime type and duration + self.chime = chime + + sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py new file mode 100644 index 0000000000..39d7c32736 --- /dev/null +++ b/selfdrive/car/gm/carstate.py @@ -0,0 +1,144 @@ +import numpy as np +from cereal import car +from common.kalman.simple_kalman import KF1D +from selfdrive.config import Conversions as CV +from selfdrive.can.parser import CANParser +from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \ + CruiseButtons, is_eps_status_ok + +def get_powertrain_can_parser(CP, canbus): + # this function generates lists for signal, messages and initial values + signals = [ + # sig_name, sig_address, default + ("BrakePedalPosition", "EBCMBrakePedalPosition", 0), + ("FrontLeftDoor", "BCMDoorBeltStatus", 0), + ("FrontRightDoor", "BCMDoorBeltStatus", 0), + ("RearLeftDoor", "BCMDoorBeltStatus", 0), + ("RearRightDoor", "BCMDoorBeltStatus", 0), + ("LeftSeatBelt", "BCMDoorBeltStatus", 0), + ("RightSeatBelt", "BCMDoorBeltStatus", 0), + ("TurnSignals", "BCMTurnSignals", 0), + ("AcceleratorPedal", "AcceleratorPedal", 0), + ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS), + ("SteeringWheelAngle", "PSCMSteeringAngle", 0), + ("FLWheelSpd", "EBCMWheelSpdFront", 0), + ("FRWheelSpd", "EBCMWheelSpdFront", 0), + ("RLWheelSpd", "EBCMWheelSpdRear", 0), + ("RRWheelSpd", "EBCMWheelSpdRear", 0), + ("PRNDL", "ECMPRDNL", 0), + ("LKADriverAppldTrq", "PSCMStatus", 0), + ("LKATorqueDeliveredStatus", "PSCMStatus", 0), + ] + + if CP.carFingerprint == CAR.VOLT: + signals += [ + ("RegenPaddle", "EBCMRegenPaddle", 0), + ("TractionControlOn", "ESPStatus", 0), + ("EPBClosed", "EPBStatus", 0), + ("CruiseMainOn", "ECMEngineStatus", 0), + ("CruiseState", "AcceleratorPedal2", 0), + ] + elif CP.carFingerprint == CAR.CADILLAC_CT6: + signals += [ + ("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0) + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain) + +class CarState(object): + def __init__(self, CP, canbus): + # initialize can parser + + self.car_fingerprint = CP.carFingerprint + self.cruise_buttons = CruiseButtons.UNPRESS + self.left_blinker_on = False + self.prev_left_blinker_on = False + self.right_blinker_on = False + self.prev_right_blinker_on = False + + # vEgo kalman filter + dt = 0.01 + self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]), + A=np.matrix([[1., dt], [0., 1.]]), + C=np.matrix([1., 0.]), + K=np.matrix([[0.12287673], [0.29666309]])) + self.v_ego = 0. + + def update(self, pt_cp): + + self.can_valid = pt_cp.can_valid + self.prev_cruise_buttons = self.cruise_buttons + self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons'] + + self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS + self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS + self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS + self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS + speed_estimate = (self.v_wheel_fl + self.v_wheel_fr + + self.v_wheel_rl + self.v_wheel_rr) / 4.0 + + self.v_ego_raw = speed_estimate + v_ego_x = self.v_ego_kf.update(speed_estimate) + self.v_ego = float(v_ego_x[0]) + self.a_ego = float(v_ego_x[1]) + + self.standstill = self.v_ego_raw < 0.01 + + self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle'] + self.gear_shifter = parse_gear_shifter(pt_cp.vl["ECMPRDNL"]['PRNDL']) + self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] + + self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] + self.user_gas_pressed = self.pedal_gas > 0 + + self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq'] + self.steer_override = abs(self.steer_torque_driver) > 1.0 + + # 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed + self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus'] + self.steer_not_allowed = not is_eps_status_ok(self.lkas_status, self.car_fingerprint) + + # 1 - open, 0 - closed + self.door_all_closed = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 0 and + pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 0 and + pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 0 and + pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 0) + + # 1 - latched + self.seatbelt = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 1 + + self.steer_error = False + + self.brake_error = False + self.can_valid = True + + self.prev_left_blinker_on = self.left_blinker_on + self.prev_right_blinker_on = self.right_blinker_on + self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1 + self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2 + + if self.car_fingerprint == CAR.VOLT: + self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed'] + self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'] + self.acc_active = False + self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1 + self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) + self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState'] + else: + self.park_brake = False + self.main_on = False + self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive'] + self.esp_disabled = False + self.regen_pressed = False + self.pcm_acc_status = int(self.acc_active) + + # Brake pedal's potentiometer returns near-zero reading + # even when pedal is not pressed. + if self.user_brake < 10: + self.user_brake = 0 + + # Regen braking is braking + self.brake_pressed = self.user_brake > 10 or self.regen_pressed + + self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive + diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py new file mode 100644 index 0000000000..0c84df422f --- /dev/null +++ b/selfdrive/car/gm/gmcan.py @@ -0,0 +1,173 @@ +def create_steering_control(packer, bus, apply_steer, idx, lkas_active): + + values = { + "LKASteeringCmdActive": lkas_active, + "LKASteeringCmd": apply_steer, + "RollingCounter": idx, + "LKASteeringCmdChecksum": 0x1000 - (lkas_active << 11) - (apply_steer & 0x7ff) - idx + } + + return packer.make_can_msg("ASCMLKASteeringCmd", bus, values) + +def create_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled): + + values = { + "LKASteeringCmdActive": 1 if enabled else 0, + "LKASteeringCmd": apply_steer, + "RollingCounter": idx, + "SetMe1": 1, + "LKASVehicleSpeed": abs(v_ego * 3.6), + "LKASMode": 2 if enabled else 0, + "LKASteeringCmdChecksum": 0 # assume zero and then manually compute it + } + + dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2] + # the checksum logic is weird + values['LKASteeringCmdChecksum'] = (0x2a + + sum([ord(i) for i in dat][:4]) + + values['LKASMode']) & 0x3ff + # pack again with checksum + dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2] + + return [0x152, 0, dat, canbus.powertrain], \ + [0x154, 0, dat, canbus.powertrain], \ + [0x151, 0, dat, canbus.chassis], \ + [0x153, 0, dat, canbus.chassis] + + +def create_adas_keepalive(bus): + dat = "\x00\x00\x00\x00\x00\x00\x00" + return [[0x409, 0, dat, bus], [0x40a, 0, dat, bus]] + +def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_stop): + values = { + "GasRegenCmdActive": acc_engaged, + "RollingCounter": idx, + "GasRegenCmdActiveInv": 1 - acc_engaged, + "GasRegenCmd": throttle, + "GasRegenFullStopActive": at_full_stop, + "GasRegenAlwaysOne": 1, + "GasRegenAlwaysOne2": 1, + "GasRegenAlwaysOne3": 1, + } + + dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2] + values["GasRegenChecksum"] = (((0xff - ord(dat[1])) & 0xff) << 16) | \ + (((0xff - ord(dat[2])) & 0xff) << 8) | \ + ((0x100 - ord(dat[3]) - idx) & 0xff) + + return packer.make_can_msg("ASCMGasRegenCmd", bus, values) + +def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_full_stop): + + if apply_brake == 0: + mode = 0x1 + else: + mode = 0xa + + if at_full_stop: + mode = 0xd + # TODO: this is to have GM bringing the car to complete stop, + # but currently it conflicts with OP controls, so turned off. + #elif near_stop: + # mode = 0xb + + brake = (0x1000 - apply_brake) & 0xfff + checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff + + values = { + "RollingCounter" : idx, + "FrictionBrakeMode" : mode, + "FrictionBrakeChecksum": checksum, + "FrictionBrakeCmd" : -apply_brake + } + + return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values) + +def create_acc_dashboard_command(bus, acc_engaged, target_speed_ms, lead_car_in_sight): + engaged = 0x90 if acc_engaged else 0 + lead_car = 0x10 if lead_car_in_sight else 0 + target_speed = int(target_speed_ms * 208) & 0xfff + speed_high = target_speed >> 8 + speed_low = target_speed & 0xff + dat = [0x01, 0x00, engaged | speed_high, speed_low, 0x01, lead_car] + return [0x370, 0, "".join(map(chr, dat)), bus] + +def create_adas_time_status(bus, tt, idx): + dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff, + ((tt & 0xf) << 4) + (idx << 2)] + chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3] + chksum = chksum & 0xfff + dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12] + return [0xa1, 0, "".join(map(chr, dat)), bus] + +def create_adas_steering_status(bus, idx): + dat = [idx << 6, 0xf0, 0x20, 0, 0, 0] + chksum = 0x60 + sum(dat) + dat += [chksum >> 8, chksum & 0xff] + return [0x306, 0, "".join(map(chr, dat)), bus] + +def create_adas_accelerometer_speed_status(bus, speed_ms, idx): + spd = int(speed_ms * 16) & 0xfff + accel = 0 & 0xfff + # 0 if in park/neutral, 0x10 if in reverse, 0x08 for D/L + #stick = 0x08 + near_range_cutoff = 0x27 + near_range_mode = 1 if spd <= near_range_cutoff else 0 + far_range_mode = 1 - near_range_mode + dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0] + chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4] + dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff] + return [0x308, 0, "".join(map(chr, dat)), bus] + +def create_adas_headlights_status(bus): + return [0x310, 0, "\x42\x04", bus] + +def create_chime_command(bus, chime_type, duration, repeat_cnt): + dat = [chime_type, duration, repeat_cnt, 0xff, 0] + return [0x10400060, 0, "".join(map(chr, dat)), bus] + +# TODO: WIP +''' +def create_friction_brake_command_ct6(packer, bus, apply_brake, idx, near_stop, at_full_stop): + + # counters loops across [0, 29, 42, 55] but checksum only considers 0, 1, 2, 3 + cntrs = [0, 29, 42, 55] + if apply_brake == 0: + mode = 0x1 + else: + mode = 0xa + + if at_full_stop: + mode = 0xd + elif near_stop: + mode = 0xb + + brake = (0x1000 - apply_brake) & 0xfff + checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff + + values = { + "RollingCounter" : cntrs[idx], + "FrictionBrakeMode" : mode, + "FrictionBrakeChecksum": checksum, + "FrictionBrakeCmd" : -apply_brake + } + + dat = packer.make_can_msg("EBCMFrictionBrakeCmd", 0, values)[2] + # msg is 0x315 but we are doing the panda forwarding + return [0x314, 0, dat, 2] + +def create_gas_regen_command_ct6(bus, throttle, idx, acc_engaged, at_full_stop): + cntrs = [0, 7, 10, 13] + eng_bit = 1 if acc_engaged else 0 + gas_high = (throttle >> 8) | 0x80 + gas_low = (throttle) & 0xff + full_stop = 0x20 if at_full_stop else 0 + + chk1 = (0x100 - gas_high - 1) & 0xff + chk2 = (0x100 - gas_low - idx) & 0xff + dat = [(idx << 6) | eng_bit, 0xc2 | full_stop, gas_high, gas_low, + (1 - eng_bit) | (cntrs[idx] << 1), 0x5d - full_stop, chk1, chk2] + return [0x2cb, 0, "".join(map(chr, dat)), bus] + +''' diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py new file mode 100755 index 0000000000..d23f138089 --- /dev/null +++ b/selfdrive/car/gm/interface.py @@ -0,0 +1,332 @@ +#!/usr/bin/env python +from cereal import car +from common.realtime import sec_since_boot +from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.car.gm.values import DBC, CAR +from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser + +try: + from selfdrive.car.gm.carcontroller import CarController +except ImportError: + CarController = None + +# Car chimes, beeps, blinker sounds etc +class CM: + TOCK = 0x81 + TICK = 0x82 + LOW_BEEP = 0x84 + HIGH_BEEP = 0x85 + LOW_CHIME = 0x86 + HIGH_CHIME = 0x87 + +# GM cars have 4 CAN buses, which creates many ways +# of how the car can be connected to. +# This ia a helper class for the interface to be setup-agnostic. +# Supports single Panda setup (connected to OBDII port), +# and a CAN forwarding setup (connected to camera module connector). + +class CanBus(object): + def __init__(self): + self.powertrain = 0 + self.obstacle = 1 + self.chassis = 2 + self.sw_gmlan = 3 + +class CarInterface(object): + def __init__(self, CP, sendcan=None): + self.CP = CP + + self.frame = 0 + self.gas_pressed_prev = False + self.brake_pressed_prev = False + self.can_invalid_count = 0 + self.acc_active_prev = 0 + + # *** init the major players *** + canbus = CanBus() + self.CS = CarState(CP, canbus) + self.VM = VehicleModel(CP) + self.pt_cp = get_powertrain_can_parser(CP, canbus) + self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis'] + + # sending if read only is False + if sendcan is not None: + self.sendcan = sendcan + self.CC = CarController(canbus, CP.carFingerprint) + + @staticmethod + def compute_gb(accel, speed): + return float(accel) / 4.0 + + @staticmethod + def calc_accel_override(a_ego, a_target, v_ego, v_target): + return 1.0 + + @staticmethod + def get_params(candidate, fingerprint): + ret = car.CarParams.new_message() + + ret.carName = "gm" + ret.carFingerprint = candidate + + ret.enableCruise = False + + # TODO: gate this on detection + ret.enableCamera = True + std_cargo = 136 + + if candidate == CAR.VOLT: + # supports stop and go, but initial engage must be above 18mph (which include conservatism) + ret.minEnableSpeed = 18 * CV.MPH_TO_MS + # kg of standard extra cargo to count for drive, gas, etc... + ret.mass = 1607 + std_cargo + ret.safetyModel = car.CarParams.SafetyModels.gm + ret.wheelbase = 2.69 + ret.steerRatio = 15.7 + ret.steerRatioRear = 0. + ret.centerToFront = ret.wheelbase * 0.4 # wild guess + + elif candidate == CAR.CADILLAC_CT6: + # engage speed is decided by pcm + ret.minEnableSpeed = -1 + # kg of standard extra cargo to count for drive, gas, etc... + ret.mass = 4016/2.205 + std_cargo + ret.safetyModel = car.CarParams.SafetyModels.cadillac + ret.wheelbase = 3.11 + ret.steerRatio = 14.6 # it's 16.3 without rear active steering + ret.steerRatioRear = 0. # TODO: there is RAS on this car! + ret.centerToFront = ret.wheelbase * 0.465 + + + # hardcoding honda civic 2016 touring params so they can be used to + # scale unknown params for other cars + mass_civic = 2923./2.205 + std_cargo + wheelbase_civic = 2.70 + centerToFront_civic = wheelbase_civic * 0.4 + centerToRear_civic = wheelbase_civic - centerToFront_civic + rotationalInertia_civic = 2500 + tireStiffnessFront_civic = 85400 + tireStiffnessRear_civic = 90000 + + centerToRear = ret.wheelbase - ret.centerToFront + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = rotationalInertia_civic * \ + ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # mass and CG position, so all cars will have approximately similar dyn behaviors + ret.tireStiffnessFront = tireStiffnessFront_civic * \ + ret.mass / mass_civic * \ + (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) + ret.tireStiffnessRear = tireStiffnessRear_civic * \ + ret.mass / mass_civic * \ + (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) + + + # same tuning for Volt and CT6 for now + ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] + ret.steerKpV, ret.steerKiV = [[0.2], [0.00]] + ret.steerKf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 + + ret.steerMaxBP = [0.] # m/s + ret.steerMaxV = [1.] + ret.gasMaxBP = [0.] + ret.gasMaxV = [.5] + ret.brakeMaxBP = [0.] + ret.brakeMaxV = [1.] + ret.longPidDeadzoneBP = [0.] + ret.longPidDeadzoneV = [0.] + + ret.longitudinalKpBP = [5., 35.] + ret.longitudinalKpV = [2.4, 1.5] + ret.longitudinalKiBP = [0.] + ret.longitudinalKiV = [0.36] + + ret.steerLimitAlert = True + + ret.stoppingControl = True + ret.startAccel = 0.8 + + ret.steerActuatorDelay = 0.1 # Default delay, not measured yet + ret.steerRateCost = 0.5 + ret.steerControlType = car.CarParams.SteerControlType.torque + + return ret + + # returns a car.CarState + def update(self, c): + + self.pt_cp.update(int(sec_since_boot() * 1e9), False) + self.CS.update(self.pt_cp) + + # create message + ret = car.CarState.new_message() + + # speeds + ret.vEgo = self.CS.v_ego + ret.aEgo = self.CS.a_ego + ret.vEgoRaw = self.CS.v_ego_raw + ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) + ret.standstill = self.CS.standstill + ret.wheelSpeeds.fl = self.CS.v_wheel_fl + ret.wheelSpeeds.fr = self.CS.v_wheel_fr + ret.wheelSpeeds.rl = self.CS.v_wheel_rl + ret.wheelSpeeds.rr = self.CS.v_wheel_rr + + # gas pedal information. + ret.gas = self.CS.pedal_gas / 254.0 + ret.gasPressed = self.CS.user_gas_pressed + + # brake pedal + ret.brake = self.CS.user_brake / 0xd0 + ret.brakePressed = self.CS.brake_pressed + + # steering wheel + ret.steeringAngle = self.CS.angle_steers + + # torque and user override. Driver awareness + # timer resets when the user uses the steering wheel. + ret.steeringPressed = self.CS.steer_override + ret.steeringTorque = self.CS.steer_torque_driver + + # cruise state + ret.cruiseState.available = bool(self.CS.main_on) + ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 + ret.cruiseState.standstill = self.CS.pcm_acc_status == 4 + + ret.leftBlinker = self.CS.left_blinker_on + ret.rightBlinker = self.CS.right_blinker_on + ret.doorOpen = not self.CS.door_all_closed + ret.seatbeltUnlatched = not self.CS.seatbelt + ret.gearShifter = self.CS.gear_shifter + + buttonEvents = [] + + # blinkers + if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: + be = car.CarState.ButtonEvent.new_message() + be.type = 'leftBlinker' + be.pressed = self.CS.left_blinker_on + buttonEvents.append(be) + + if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: + be = car.CarState.ButtonEvent.new_message() + be.type = 'rightBlinker' + be.pressed = self.CS.right_blinker_on + buttonEvents.append(be) + + if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: + be = car.CarState.ButtonEvent.new_message() + be.type = 'unknown' + if self.CS.cruise_buttons != CruiseButtons.UNPRESS: + be.pressed = True + but = self.CS.cruise_buttons + else: + be.pressed = False + but = self.CS.prev_cruise_buttons + if but == CruiseButtons.RES_ACCEL: + be.type = 'accelCruise' + elif but == CruiseButtons.DECEL_SET: + be.type = 'decelCruise' + elif but == CruiseButtons.CANCEL: + be.type = 'cancel' + elif but == CruiseButtons.MAIN: + be.type = 'altButton3' + buttonEvents.append(be) + + ret.buttonEvents = buttonEvents + + events = [] + if not self.CS.can_valid: + self.can_invalid_count += 1 + if self.can_invalid_count >= 5: + events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + else: + self.can_invalid_count = 0 + if self.CS.steer_error: + events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) + if self.CS.steer_not_allowed: + events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) + if ret.doorOpen: + events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if ret.seatbeltUnlatched: + events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + + if self.CS.car_fingerprint == CAR.VOLT: + + if self.CS.brake_error: + events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) + if not self.CS.gear_shifter_valid: + events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if self.CS.esp_disabled: + events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if not self.CS.main_on: + events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) + if self.CS.gear_shifter == 3: + events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if ret.vEgo < self.CP.minEnableSpeed: + events.append(create_event('speedTooLow', [ET.NO_ENTRY])) + if self.CS.park_brake: + events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) + # disable on pedals rising edge or when brake is pressed and speed isn't zero + if (ret.gasPressed and not self.gas_pressed_prev) or \ + (ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + if ret.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + if ret.cruiseState.standstill: + events.append(create_event('resumeRequired', [ET.WARNING])) + + # handle button presses + for b in ret.buttonEvents: + # do enable on both accel and decel buttons + if b.type in ["accelCruise", "decelCruise"] and not b.pressed: + events.append(create_event('buttonEnable', [ET.ENABLE])) + # do disable on button down + if b.type == "cancel" and b.pressed: + events.append(create_event('buttonCancel', [ET.USER_DISABLE])) + + if self.CS.car_fingerprint == CAR.CADILLAC_CT6: + + if self.CS.acc_active and not self.acc_active_prev: + events.append(create_event('pcmEnable', [ET.ENABLE])) + if not self.CS.acc_active: + events.append(create_event('pcmDisable', [ET.USER_DISABLE])) + + ret.events = events + + # update previous brake/gas pressed + self.acc_active_prev = self.CS.acc_active + self.gas_pressed_prev = ret.gasPressed + self.brake_pressed_prev = ret.brakePressed + + # cast to reader so it can't be modified + return ret.as_reader() + + # pass in a car.CarControl + # to be called @ 100hz + def apply(self, c): + hud_v_cruise = c.hudControl.setSpeed + if hud_v_cruise > 70: + hud_v_cruise = 0 + + chime, chime_count = { + "none": (0, 0), + "beepSingle": (CM.HIGH_CHIME, 1), + "beepTriple": (CM.HIGH_CHIME, 3), + "beepRepeated": (CM.LOW_CHIME, -1), + "chimeSingle": (CM.LOW_CHIME, 1), + "chimeDouble": (CM.LOW_CHIME, 2), + "chimeRepeated": (CM.LOW_CHIME, -1), + "chimeContinuous": (CM.LOW_CHIME, -1)}[str(c.hudControl.audibleAlert)] + + self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \ + c.actuators, + hud_v_cruise, c.hudControl.lanesVisible, \ + c.hudControl.leadVisible, \ + chime, chime_count) + + self.frame += 1 diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py new file mode 100755 index 0000000000..bc35b3459b --- /dev/null +++ b/selfdrive/car/gm/radar_interface.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python +import zmq +import math +import time +import numpy as np +from cereal import car +from selfdrive.can.parser import CANParser +from selfdrive.car.gm.interface import CanBus +from selfdrive.car.gm.values import DBC, CAR +from common.realtime import sec_since_boot +from selfdrive.services import service_list +import selfdrive.messaging as messaging + +NUM_TARGETS_MSG = 1120 +SLOT_1_MSG = NUM_TARGETS_MSG + 1 +NUM_SLOTS = 20 + +# Actually it's 0x47f, but can parser only reports +# messages that are present in DBC +LAST_RADAR_MSG = NUM_TARGETS_MSG + NUM_SLOTS + +def create_radard_can_parser(canbus, car_fingerprint): + + dbc_f = DBC[car_fingerprint]['radar'] + if car_fingerprint == CAR.VOLT: + # C1A-ARS3-A by Continental + radar_targets = range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS) + signals = zip(['LRRNumObjects'] + + ['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS + + ['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS + + ['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS, + [NUM_TARGETS_MSG] + radar_targets * 6, + [0] + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + + [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + + [0.0] * NUM_SLOTS + [0] * NUM_SLOTS) + + checks = [] + + return CANParser(dbc_f, signals, checks, canbus.obstacle) + else: + return None + +class RadarInterface(object): + def __init__(self, CP): + # radar + self.pts = {} + self.track_id = 0 + self.num_targets = 0 + + self.delay = 0.0 # Delay of radar + + canbus = CanBus() + print "Using %d as obstacle CAN bus ID" % canbus.obstacle + self.rcp = create_radard_can_parser(canbus, CP.carFingerprint) + + context = zmq.Context() + self.logcan = messaging.sub_sock(context, service_list['can'].port) + + def update(self): + updated_messages = set() + ret = car.RadarState.new_message() + while 1: + + if self.rcp is None: + time.sleep(0.05) # nothing to do + return ret + + tm = int(sec_since_boot() * 1e9) + updated_messages.update(self.rcp.update(tm, True)) + if LAST_RADAR_MSG in updated_messages: + break + + errors = [] + if not self.rcp.can_valid: + errors.append("notValid") + ret.errors = errors + + currentTargets = set() + if self.rcp.vl[NUM_TARGETS_MSG]['LRRNumObjects'] != self.num_targets: + self.num_targets = self.rcp.vl[NUM_TARGETS_MSG]['LRRNumObjects'] + + # Not all radar messages describe targets, + # no need to monitor all of the sself.rcp.msgs_upd + for ii in updated_messages: + if ii == NUM_TARGETS_MSG: + continue + + if self.num_targets == 0: + break + + cpt = self.rcp.vl[ii] + # Zero distance means it's an empty target slot + if cpt['TrkRange'] > 0.0: + targetId = cpt['TrkObjectID'] + currentTargets.add(targetId) + if targetId not in self.pts: + self.pts[targetId] = car.RadarState.RadarPoint.new_message() + self.pts[targetId].trackId = targetId + distance = cpt['TrkRange'] + self.pts[targetId].dRel = distance # from front of car + # From driver's pov, left is positive + deg_to_rad = np.pi/180. + self.pts[targetId].yRel = math.sin(deg_to_rad * cpt['TrkAzimuth']) * distance + self.pts[targetId].vRel = cpt['TrkRangeRate'] + self.pts[targetId].aRel = float('nan') + self.pts[targetId].yvRel = float('nan') + + for oldTarget in self.pts.keys(): + if not oldTarget in currentTargets: + del self.pts[oldTarget] + + ret.points = self.pts.values() + return ret + +if __name__ == "__main__": + RI = RadarInterface(None) + while 1: + ret = RI.update() + print(chr(27) + "[2J") + print ret + + diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py new file mode 100644 index 0000000000..9cbcbbbf97 --- /dev/null +++ b/selfdrive/car/gm/values.py @@ -0,0 +1,48 @@ +from cereal import car +from selfdrive.car import dbc_dict + +class CAR: + VOLT = "CHEVROLET VOLT PREMIER 2017" + CADILLAC_CT6 = "CADILLAC CT6 SUPERCRUISE 2018" + +class CruiseButtons: + UNPRESS = 1 + RES_ACCEL = 2 + DECEL_SET = 3 + MAIN = 5 + CANCEL = 6 + +def is_eps_status_ok(eps_status, car_fingerprint): + valid_eps_status = [] + if car_fingerprint == CAR.VOLT: + valid_eps_status += [0, 1] + elif car_fingerprint == CAR.CADILLAC_CT6: + valid_eps_status += [0, 1, 4, 5, 6] + return eps_status in valid_eps_status + +def parse_gear_shifter(can_gear): + if can_gear == 0: + return car.CarState.GearShifter.park + elif can_gear == 1: + return car.CarState.GearShifter.neutral + elif can_gear == 2: + return car.CarState.GearShifter.drive + elif can_gear == 3: + return car.CarState.GearShifter.reverse + else: + return car.CarState.GearShifter.unknown + +FINGERPRINTS = { + CAR.VOLT: [{ + 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 + }], + CAR.CADILLAC_CT6: [{ + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 336: 1, 338: 6, 340: 6, 352: 5, 354: 5, 356: 8, 368: 3, 372: 5, 381: 8, 386: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 458: 5, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 800: 6, 801: 8, 804: 3, 810: 8, 832: 8, 833: 8, 834: 8, 835: 6, 836: 5, 837: 8, 838: 8, 839: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 884: 8, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 1, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1934: 7, 2016: 8, 2024: 8 + }], +} + + +DBC = { + CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.CADILLAC_CT6: dbc_dict('cadillac_ct6_powertrain', 'cadillac_ct6_object', chassis_dbc='cadillac_ct6_chassis'), +} diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 10f98f59eb..c47972e177 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -1,11 +1,10 @@ -from collections import namedtuple import os +from collections import namedtuple from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.controls.lib.drive_helpers import rate_limit from common.numpy_fast import clip -from . import hondacan -from .values import AH -from common.fingerprints import HONDA as CAR +from selfdrive.car.honda import hondacan +from selfdrive.car.honda.values import AH, CruiseButtons, CAR from selfdrive.can.packer import CANPacker @@ -101,6 +100,10 @@ class CarController(object): else: hud_car = 0 + # For lateral control-only, send chimes as a beep since we don't send 0x1fa + if CS.CP.radarOffCan: + snd_beep = snd_beep if snd_beep is not 0 else snd_chime + #print chime, alert_id, hud_alert fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) @@ -115,13 +118,13 @@ class CarController(object): # *** compute control surfaces *** BRAKE_MAX = 1024/4 - if CS.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.PILOT, CAR.RIDGELINE): - is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] - STEER_MAX = 0x1FFF if is_fw_modified else 0x1000 + if CS.CP.carFingerprint in (CAR.ACURA_ILX): + STEER_MAX = 0xF00 elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) else: - STEER_MAX = 0xF00 + is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] + STEER_MAX = 0x1FFF if is_fw_modified else 0x1000 # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) @@ -137,32 +140,39 @@ class CarController(object): # Send steering command. idx = frame % 4 - can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CS.CP.carFingerprint, idx)) - - # Send gas and brake commands. - if (frame % 2) == 0: - idx = (frame / 2) % 4 - can_sends.append( - hondacan.create_brake_command(self.packer, apply_brake, pcm_override, - pcm_cancel_cmd, hud.chime, hud.fcw, idx)) - if CS.CP.enableGasInterceptor: - # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. - # This prevents unexpected pedal range rescaling - can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx)) + can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, enabled, CS.CP.carFingerprint, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame/10) % 4 can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx)) - # radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug) - if CS.CP.carFingerprint == CAR.ACURA_ILX: - radar_send_step = 2 + if CS.CP.radarOffCan: + # If using stock ACC, spam cancel command to kill gas when OP disengages. + if pcm_cancel_cmd: + can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx)) + elif CS.stopped: + can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx)) else: - radar_send_step = 5 + # Send gas and brake commands. + if (frame % 2) == 0: + idx = (frame / 2) % 4 + can_sends.append( + hondacan.create_brake_command(self.packer, apply_brake, pcm_override, + pcm_cancel_cmd, hud.chime, hud.fcw, idx)) + if CS.CP.enableGasInterceptor: + # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. + # This prevents unexpected pedal range rescaling + can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx)) + + # radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug) + if CS.CP.carFingerprint == CAR.ACURA_ILX: + radar_send_step = 2 + else: + radar_send_step = 5 - if (frame % radar_send_step) == 0: - idx = (frame/radar_send_step) % 4 - can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, idx)) + if (frame % radar_send_step) == 0: + idx = (frame/radar_send_step) % 4 + can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, idx)) sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes()) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 54fee0940c..88cc6d7b32 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,9 +1,8 @@ -import os from common.numpy_fast import interp +from common.kalman.simple_kalman import KF1D from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV -from common.kalman.simple_kalman import KF1D -from common.fingerprints import HONDA as CAR +from selfdrive.car.honda.values import CAR, DBC import numpy as np @@ -21,7 +20,7 @@ def parse_gear_shifter(can_gear_shifter, car_fingerprint): return "drive" elif can_gear_shifter == 0xa: return "sport" - elif car_fingerprint in (CAR.CIVIC, CAR.CRV, CAR.ACURA_RDX): + elif car_fingerprint in (CAR.CIVIC, CAR.CRV, CAR.ACURA_RDX, CAR.CRV_5G, CAR.CIVIC_HATCH): if can_gear_shifter == 0x1: return "park" elif can_gear_shifter == 0x2: @@ -35,7 +34,7 @@ def parse_gear_shifter(can_gear_shifter, car_fingerprint): elif can_gear_shifter == 0x20: return "low" - elif car_fingerprint in (CAR.PILOT, CAR.RIDGELINE): + elif car_fingerprint in (CAR.ACCORD, CAR.PILOT, CAR.RIDGELINE): if can_gear_shifter == 0x8: return "reverse" elif can_gear_shifter == 0x4: @@ -71,18 +70,12 @@ def get_can_signals(CP): ("STEER_ANGLE", "STEERING_SENSORS", 0), ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0), ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0), - ("DOOR_OPEN_FL", "DOORS_STATUS", 1), - ("DOOR_OPEN_FR", "DOORS_STATUS", 1), - ("DOOR_OPEN_RL", "DOORS_STATUS", 1), - ("DOOR_OPEN_RR", "DOORS_STATUS", 1), ("LEFT_BLINKER", "SCM_FEEDBACK", 0), ("RIGHT_BLINKER", "SCM_FEEDBACK", 0), - ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0), ("GEAR", "GEARBOX", 0), ("WHEELS_MOVING", "STANDSTILL", 1), ("BRAKE_ERROR_1", "STANDSTILL", 1), ("BRAKE_ERROR_2", "STANDSTILL", 1), - ("CRUISE_SPEED_PCM", "CRUISE", 0), ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1), ("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS", 0), ("BRAKE_PRESSED", "POWERTRAIN_DATA", 0), @@ -102,9 +95,7 @@ def get_can_signals(CP): ("ENGINE_DATA", 100), ("WHEEL_SPEEDS", 50), ("STEERING_SENSORS", 100), - ("DOORS_STATUS", 3), ("SCM_FEEDBACK", 10), - ("CRUISE_PARAMS", 10), ("GEARBOX", 100), ("STANDSTILL", 50), ("SEATBELT_STATUS", 10), @@ -114,34 +105,52 @@ def get_can_signals(CP): ("SCM_BUTTONS", 25), ] + if CP.radarOffCan: + # Civic is only bosch to use the same brake message as other hondas. + if CP.carFingerprint != CAR.CIVIC_HATCH: + signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)] + checks += [("BRAKE_MODULE", 50)] + signals += [("CAR_GAS", "GAS_PEDAL_2", 0), + ("MAIN_ON", "SCM_FEEDBACK", 0), + ("EPB_STATE", "EPB_STATUS", 0), + ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0), + ("CRUISE_SPEED", "ACC_HUD", 0)] + checks += [("GAS_PEDAL_2", 100)] + else: + # Nidec signals. + signals += [("CRUISE_SPEED_PCM", "CRUISE", 0), + ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)] + checks += [("CRUISE_PARAMS", 50)] + + if CP.carFingerprint == CAR.ACCORD: + signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)] + else: + signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1), + ("DOOR_OPEN_FR", "DOORS_STATUS", 1), + ("DOOR_OPEN_RL", "DOORS_STATUS", 1), + ("DOOR_OPEN_RR", "DOORS_STATUS", 1)] + checks += [("DOORS_STATUS", 3)] if CP.carFingerprint == CAR.CIVIC: - dbc_f = 'honda_civic_touring_2016_can_generated.dbc' signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_FEEDBACK", 0), ("EPB_STATE", "EPB_STATUS", 0), ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)] elif CP.carFingerprint == CAR.ACURA_ILX: - dbc_f = 'acura_ilx_2016_can_generated.dbc' signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_BUTTONS", 0)] elif CP.carFingerprint == CAR.CRV: - dbc_f = 'honda_crv_touring_2016_can_generated.dbc' signals += [("MAIN_ON", "SCM_BUTTONS", 0)] elif CP.carFingerprint == CAR.ACURA_RDX: - dbc_f = 'acura_rdx_2018_can_generated.dbc' signals += [("MAIN_ON", "SCM_BUTTONS", 0)] elif CP.carFingerprint == CAR.ODYSSEY: - dbc_f = 'honda_odyssey_exl_2018_generated.dbc' signals += [("MAIN_ON", "SCM_FEEDBACK", 0), ("EPB_STATE", "EPB_STATUS", 0), ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)] checks += [("EPB_STATUS", 50)] elif CP.carFingerprint == CAR.PILOT: - dbc_f = 'honda_pilot_touring_2017_can_generated.dbc' signals += [("MAIN_ON", "SCM_BUTTONS", 0), ("CAR_GAS", "GAS_PEDAL_2", 0)] elif CP.carFingerprint == CAR.RIDGELINE: - dbc_f = 'honda_ridgeline_black_edition_2017_can_generated.dbc' signals += [("MAIN_ON", "SCM_BUTTONS", 0)] # add gas interceptor reading if we are using it @@ -149,12 +158,12 @@ def get_can_signals(CP): signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) checks.append(("GAS_SENSOR", 50)) - return dbc_f, signals, checks + return signals, checks def get_can_parser(CP): - dbc_f, signals, checks = get_can_signals(CP) - return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0) + signals, checks = get_can_signals(CP) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) class CarState(object): @@ -167,11 +176,14 @@ class CarState(object): self.cruise_buttons = 0 self.cruise_setting = 0 + self.v_cruise_pcm_prev = 0 self.blinker_on = 0 self.left_blinker_on = 0 self.right_blinker_on = 0 + self.stopped = 0 + # vEgo kalman filter dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) @@ -200,14 +212,18 @@ class CarState(object): self.prev_right_blinker_on = self.right_blinker_on # ******************* parse out can ******************* - self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], - cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) + if self.CP.carFingerprint in (CAR.ACCORD): + self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'] + else: + self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], + cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'] # 2 = temporary 3= TBD 4 = temporary, hit a bump 5 (permanent) 6 = temporary 7 (permanent) # TODO: Use values from DBC to parse this field self.steer_error = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 2, 3, 4, 6] self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] != 0 + self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 3] # 3 is low speed lockout, not worth a warning self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2'] self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED'] @@ -248,17 +264,15 @@ class CarState(object): self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] - if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY): + if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.CIVIC_HATCH): self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE'] self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] else: self.park_brake = 0 # TODO self.brake_hold = 0 # TODO - self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] - self.cruise_speed_offset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego) self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.CP.carFingerprint) self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS'] @@ -275,18 +289,37 @@ class CarState(object): self.steer_override = abs(cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) > 1200 self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] - # brake switch has shown some single time step noise, so only considered when - # switch is on for at least 2 consecutive CAN samples self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] - self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ + + if self.CP.radarOffCan: + self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252. + self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego) + if self.CP.carFingerprint == CAR.CIVIC_HATCH: + self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ + (self.brake_switch and self.brake_switch_prev and \ + cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) + self.brake_switch_prev = self.brake_switch + self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + else: + self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] + # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this. + self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED'] + self.v_cruise_pcm_prev = self.v_cruise_pcm + else: + self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + self.cruise_speed_offset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego) + self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM'] + # brake switch has shown some single time step noise, so only considered when + # switch is on for at least 2 consecutive CAN samples + self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ (self.brake_switch and self.brake_switch_prev and \ cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) - self.brake_switch_prev = self.brake_switch - self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + self.brake_switch_prev = self.brake_switch + self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE'] self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] - self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM'] self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD'] diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 66ccbff5f0..a5426453db 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -2,7 +2,7 @@ import struct import common.numpy_fast as np from selfdrive.config import Conversions as CV -from common.fingerprints import HONDA as CAR +from selfdrive.car.honda.values import CAR # *** Honda specific *** def can_cksum(mm): @@ -63,30 +63,37 @@ def create_gas_command(packer, gas_amount, idx): return packer.make_can_msg("GAS_COMMAND", 0, values, idx) -def create_steering_control(packer, apply_steer, car_fingerprint, idx): +def create_steering_control(packer, apply_steer, enabled, car_fingerprint, idx): """Creates a CAN message for the Honda DBC STEERING_CONTROL.""" values = { "STEER_TORQUE": apply_steer, - "STEER_TORQUE_REQUEST": apply_steer != 0, + "STEER_TORQUE_REQUEST": enabled, } - return packer.make_can_msg("STEERING_CONTROL", 0, values, idx) + # Set bus 2 for accord and new crv. + bus = 2 if car_fingerprint in (CAR.CRV_5G, CAR.ACCORD, CAR.CIVIC_HATCH) else 0 + return packer.make_can_msg("STEERING_CONTROL", bus, values, idx) def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx): """Creates an iterable of CAN messages for the UIs.""" commands = [] + bus = 0 - acc_hud_values = { - 'PCM_SPEED': pcm_speed * CV.MS_TO_KPH, - 'PCM_GAS': hud.pcm_accel, - 'CRUISE_SPEED': hud.v_cruise, - 'ENABLE_MINI_CAR': hud.mini_car, - 'HUD_LEAD': hud.car, - 'SET_ME_X03': 0x03, - 'SET_ME_X03_2': 0x03, - 'SET_ME_X01': 0x01, - } - commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx)) + # Bosch sends commands to bus 2. + if car_fingerprint in (CAR.CRV_5G, CAR.ACCORD, CAR.CIVIC_HATCH): + bus = 2 + else: + acc_hud_values = { + 'PCM_SPEED': pcm_speed * CV.MS_TO_KPH, + 'PCM_GAS': hud.pcm_accel, + 'CRUISE_SPEED': hud.v_cruise, + 'ENABLE_MINI_CAR': hud.mini_car, + 'HUD_LEAD': hud.car, + 'SET_ME_X03': 0x03, + 'SET_ME_X03_2': 0x03, + 'SET_ME_X01': 0x01, + } + commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx)) lkas_hud_values = { 'SET_ME_X41': 0x41, @@ -95,7 +102,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx): 'SOLID_LANES': hud.lanes, 'BEEP': hud.beep, } - commands.append(packer.make_can_msg('LKAS_HUD', 0, lkas_hud_values, idx)) + commands.append(packer.make_can_msg('LKAS_HUD', bus, lkas_hud_values, idx)) if car_fingerprint in (CAR.CIVIC, CAR.ODYSSEY): commands.append(packer.make_can_msg('HIGHBEAM_CONTROL', 0, {'HIGHBEAMS_ON': False}, idx)) @@ -140,3 +147,10 @@ def create_radar_commands(v_ego, car_fingerprint, idx): commands.append(make_can_msg(0x301, msg_0x301, idx, 1)) return commands + +def spam_buttons_command(packer, button_val, idx): + values = { + 'CRUISE_BUTTONS': button_val, + 'CRUISE_SETTING': 0, + } + return packer.make_can_msg("SCM_BUTTONS", 0, values, idx) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index c50c0de9ed..b2b68bc33d 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -8,12 +8,11 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.honda.carstate import CarState, get_can_parser -from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH +from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH, CAR from selfdrive.controls.lib.planner import A_ACC_MAX -from common.fingerprints import HONDA as CAR try: - from .carcontroller import CarController + from selfdrive.car.honda.carcontroller import CarController except ImportError: CarController = None @@ -144,10 +143,14 @@ class CarInterface(object): ret.carName = "honda" ret.carFingerprint = candidate - ret.safetyModel = car.CarParams.SafetyModels.honda - - ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) - ret.enableGasInterceptor = 0x201 in fingerprint + if 0x1ef in fingerprint: + ret.safetyModel = car.CarParams.SafetyModels.hondaBosch + ret.enableCamera = True + ret.radarOffCan = True + else: + ret.safetyModel = car.CarParams.SafetyModels.honda + ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) + ret.enableGasInterceptor = 0x201 in fingerprint print "ECU Camera Simulated: ", ret.enableCamera print "ECU Gas Interceptor: ", ret.enableGasInterceptor @@ -178,6 +181,31 @@ class CarInterface(object): ret.longitudinalKpV = [3.6, 2.4, 1.5] ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.54, 0.36] + elif candidate == CAR.CIVIC_HATCH: + stop_and_go = True + ret.mass = 2916. * CV.LB_TO_KG + std_cargo + ret.wheelbase = wheelbase_civic + ret.centerToFront = centerToFront_civic + ret.steerRatio = 10.93 + ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] + + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [1.2, 0.8, 0.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.ACCORD: + stop_and_go = True + ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg + ret.mass = 3279. * CV.LB_TO_KG + std_cargo + ret.wheelbase = 2.83 + ret.centerToFront = ret.wheelbase * 0.39 + ret.steerRatio = 11.82 + ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] + + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [1.2, 0.8, 0.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.18, 0.12] elif candidate == CAR.ACURA_ILX: stop_and_go = False ret.mass = 3095 * CV.LB_TO_KG + std_cargo @@ -200,6 +228,19 @@ class CarInterface(object): ret.steerRatio = 15.3 ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] + ret.longitudinalKpBP = [0., 5., 35.] + ret.longitudinalKpV = [1.2, 0.8, 0.5] + ret.longitudinalKiBP = [0., 35.] + ret.longitudinalKiV = [0.18, 0.12] + elif candidate == CAR.CRV_5G: + stop_and_go = True + ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg + ret.mass = 3410. * CV.LB_TO_KG + std_cargo + ret.wheelbase = 2.66 + ret.centerToFront = ret.wheelbase * 0.41 + ret.steerRatio = 12.30 + ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] + ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] @@ -298,6 +339,7 @@ class CarInterface(object): ret.steerLimitAlert = True ret.startAccel = 0.5 + ret.steerActuatorDelay = 0.09 ret.steerRateCost = 0.5 return ret @@ -423,7 +465,7 @@ class CarInterface(object): self.can_invalid_count = 0 if self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - elif self.CS.steer_not_allowed: + elif self.CS.steer_warning: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 7cf4c7d61c..3081474487 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -1,12 +1,10 @@ #!/usr/bin/env python import os - -from selfdrive.can.parser import CANParser - +import zmq +import time from cereal import car +from selfdrive.can.parser import CANParser from common.realtime import sec_since_boot - -import zmq from selfdrive.services import service_list import selfdrive.messaging as messaging @@ -25,11 +23,12 @@ def _create_nidec_can_parser(): class RadarInterface(object): - def __init__(self): + def __init__(self, CP): # radar self.pts = {} self.track_id = 0 self.radar_fault = False + self.radar_off_can = CP.radarOffCan self.delay = 0.1 # Delay of radar @@ -43,6 +42,14 @@ class RadarInterface(object): canMonoTimes = [] updated_messages = set() + ret = car.RadarState.new_message() + + # in Bosch radar and we are only steering for now, so sleep 0.05s to keep + # radard at 20Hz and return no points + if self.radar_off_can: + time.sleep(0.05) + return ret + while 1: tm = int(sec_since_boot() * 1e9) updated_messages.update(self.rcp.update(tm, True)) @@ -69,7 +76,6 @@ class RadarInterface(object): if ii in self.pts: del self.pts[ii] - ret = car.RadarState.new_message() errors = [] if not self.rcp.can_valid: errors.append("commIssue") @@ -79,11 +85,15 @@ class RadarInterface(object): ret.canMonoTimes = canMonoTimes ret.points = self.pts.values() + return ret if __name__ == "__main__": - RI = RadarInterface() + class CarParams: + radarOffCan = False + + RI = RadarInterface(CarParams) while 1: ret = RI.update() print(chr(27) + "[2J") diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 35db0c2177..8d8bd3b33f 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -1,3 +1,5 @@ +from selfdrive.car import dbc_dict + # Car button codes class CruiseButtons: RES_ACCEL = 4 @@ -32,3 +34,75 @@ class AH: GEAR_NOT_D = [4, 6] SEATBELT = [5, 5] SPEED_TOO_HIGH = [6, 8] + + +class CAR: + ACCORD = "HONDA ACCORD 2018 SPORT 2T" + CIVIC = "HONDA CIVIC 2016 TOURING" + CIVIC_HATCH = "HONDA CIVIC HATCHBACK 2017 EX" + ACURA_ILX = "ACURA ILX 2016 ACURAWATCH PLUS" + CRV = "HONDA CR-V 2016 TOURING" + CRV_5G = "HONDA CR-V 2017 EX" + ODYSSEY = "HONDA ODYSSEY 2018 EX-L" + ACURA_RDX = "ACURA RDX 2018 ACURAWATCH PLUS" + PILOT = "HONDA PILOT 2017 TOURING" + RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION" + + +FINGERPRINTS = { + CAR.ACCORD: [{ + 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8 + }], + CAR.ACURA_ILX: [{ + 1024L: 5, 513L: 6, 1027L: 5, 1029L: 8, 929L: 4, 1057L: 5, 777L: 8, 1034L: 5, 1036L: 8, 398L: 3, 399L: 7, 145L: 8, 660L: 8, 985L: 3, 923L: 2, 542L: 7, 773L: 7, 800L: 8, 432L: 7, 419L: 8, 420L: 8, 1030L: 5, 422L: 8, 808L: 8, 428L: 8, 304L: 8, 819L: 7, 821L: 5, 57L: 3, 316L: 8, 545L: 4, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 1039L: 8, 476L: 4, 892L: 8, 490L: 8, 1064L: 7, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 380L: 8, 1365L: 5, + # sent messages + 0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5, + }], + CAR.ACURA_RDX: [{ + 57L: 3, 145L: 8, 229L: 4, 308L: 5, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 392L: 6, 398L: 3, 399L: 6, 404L: 4, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 892L: 8, 923L: 2, 929L: 4, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1034L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1365L: 5, 1424L: 5, 1729L: 1 + }], + CAR.CIVIC: [{ + 1024L: 5, 513L: 6, 1027L: 5, 1029L: 8, 777L: 8, 1036L: 8, 1039L: 8, 1424L: 5, 401L: 8, 148L: 8, 662L: 4, 985L: 3, 795L: 8, 773L: 7, 800L: 8, 545L: 6, 420L: 8, 806L: 8, 808L: 8, 1322L: 5, 427L: 3, 428L: 8, 304L: 8, 432L: 7, 57L: 3, 450L: 8, 929L: 8, 330L: 8, 1302L: 8, 464L: 8, 1361L: 5, 1108L: 8, 597L: 8, 470L: 2, 344L: 8, 804L: 8, 399L: 7, 476L: 7, 1633L: 8, 487L: 4, 892L: 8, 490L: 8, 493L: 5, 884L: 8, 891L: 8, 380L: 8, 1365L: 5, + # sent messages + 0xe4: 5, 0x1fa: 8, 0x200: 6, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8, + }], + CAR.CIVIC_HATCH: [{ + 57L: 3, 148L: 8, 228L: 5, 304L: 8, 330L: 8, 344L: 8, 380L: 8, 399L: 7, 401L: 8, 420L: 8, 427L: 3, 428L: 8, 432L: 7, 441L: 5, 450L: 8, 464L: 8, 470L: 2, 476L: 7, 477L: 8, 479L: 8, 490L: 8, 493L: 5, 495L: 8, 506L: 8, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 829L: 5, 862L: 8, 884L: 8, 891L: 8, 892L: 8, 927L: 8, 929L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1036L: 8, 1039L: 8, 1108L: 8, 1302L: 8, 1322L: 5, 1361L: 5, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, 1633L: 8 + }], + CAR.CRV: [{ + 57L: 3, 145L: 8, 316L: 8, 340L: 8, 342L: 6, 344L: 8, 380L: 8, 398L: 3, 399L: 6, 401L: 8, 420L: 8, 422L: 8, 426L: 8, 432L: 7, 464L: 8, 474L: 5, 476L: 4, 487L: 4, 490L: 8, 493L: 3, 507L: 1, 542L: 7, 545L: 4, 597L: 8, 660L: 8, 661L: 4, 773L: 7, 777L: 8, 800L: 8, 804L: 8, 808L: 8, 882L: 2, 884L: 7, 888L: 8, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1033L: 5, 1036L: 8, 1039L: 8, 1057L: 5, 1064L: 7, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, + # sent messages + 0x194: 4, 0x1fa: 8, 0x30c: 8, 0x33d: 5, + }], + CAR.CRV_5G: [{ + 57L: 3, 148L: 8, 199L: 4, 228L: 5, 231L: 5, 232L: 7, 304L: 8, 330L: 8, 340L: 8, 344L: 8, 380L: 8, 399L: 7, 401L: 8, 420L: 8, 423L: 2, 427L: 3, 428L: 8, 432L: 7, 441L: 5, 446L: 3, 450L: 8, 464L: 8, 467L: 2, 469L: 3, 470L: 2, 474L: 8, 476L: 7, 477L: 8, 479L: 8, 490L: 8, 493L: 5, 495L: 8, 507L: 1, 545L: 6, 597L: 8, 661L: 4, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 814L: 4, 815L: 8, 817L: 4, 825L: 4, 829L: 5, 862L: 8, 881L: 8, 882L: 4, 884L: 8, 888L: 8, 891L: 8, 927L: 8, 918L: 7, 929L: 8, 983L: 8, 985L: 3, 1024L: 5, 1027L: 5, 1029L: 8, 1036L: 8, 1039L: 8, 1064L: 7, 1108L: 8, 1092L: 1, 1115L: 4, 1125L: 8, 1127L: 2, 1296L: 8, 1302L: 8, 1322L: 5, 1361L: 5, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, 1618L: 5, 1633L: 8, 1670L: 5 + }], + CAR.ODYSSEY: [{ + 57L: 3, 148L: 8, 228L: 5, 229L: 4, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1615L: 8, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5 + }, + # Odyssey Elite + { + 57L: 3, 148L: 8, 228L: 5, 229L: 4, 304L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 440L: 8, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 507L: 1, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5 + }], + # Includes 2017 Touring and 2016 EX-L messaging. + CAR.PILOT: [{ + 57L: 3, 145L: 8, 228L: 5, 229L: 4, 308L: 5, 316L: 8, 334L: 8, 339L: 7, 342L: 6, 344L: 8, 379L: 8, 380L: 8, 392L: 6, 399L: 7, 419L: 8, 420L: 8, 422L: 8, 425L: 8, 426L: 8, 427L: 3, 432L: 7, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 507L: 1, 538L: 3, 542L: 7, 545L: 5, 546L: 3, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 837L: 5, 856L: 7, 871L: 8, 882L: 2, 884L: 7, 891L: 8, 892L: 8, 923L: 2, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1027L: 5, 1029L: 8, 1036L: 8, 1039L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1108L: 8, 1125L: 8, 1296L: 8, 1424L: 5, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1616L: 5, 1618L: 5, 1668L: 5 + }], + CAR.RIDGELINE: [{ + 57L: 3, 145L: 8, 228L: 5, 229L: 4, 308L: 5, 316L: 8, 339L: 7, 342L: 6, 344L: 8, 380L: 8, 392L: 6, 399L: 7, 419L: 8, 420L: 8, 422L: 8, 425L: 8, 426L: 8, 427L: 3, 432L: 7, 464L: 8, 471L: 3, 476L: 4, 490L: 8, 506L: 8, 545L: 5, 546L: 3, 597L: 8, 660L: 8, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 808L: 8, 819L: 7, 821L: 5, 829L: 5, 871L: 8, 882L: 2, 884L: 7, 892L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1027L: 5, 1029L: 8, 1036L: 8, 1039L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1108L: 8, 1125L: 8, 1296L: 8, 1365L: 5, 1424L: 5, 1600L: 5, 1601L: 8, 1613L: 5, 1616L: 5, 1618L: 5, 1668L: 5, 2015L: 3 + }] +} + + +DBC = { + CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None), + CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'), + CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.CIVIC_HATCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), + CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None), + CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), + CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), + CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'), +} diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 15bef3fd94..ffb7907500 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -71,6 +71,7 @@ class CarInterface(object): ret.longitudinalKpV = [0.] ret.longitudinalKiBP = [0.] ret.longitudinalKiV = [0.] + ret.steerActuatorDelay = 0. return ret diff --git a/selfdrive/car/mock/values.py b/selfdrive/car/mock/values.py new file mode 100644 index 0000000000..0dd91565bd --- /dev/null +++ b/selfdrive/car/mock/values.py @@ -0,0 +1,2 @@ +class CAR: + MOCK = 'mock' diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 3c9a54bb58..1168c01fc3 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,5 +1,4 @@ -import os -from common.fingerprints import TOYOTA as CAR +from selfdrive.car.toyota.values import CAR, DBC from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV from common.kalman.simple_kalman import KF1D @@ -80,20 +79,10 @@ def get_can_parser(CP): ("EPS_STATUS", 25), ] - # this function generates lists for signal, messages and initial values if CP.carFingerprint == CAR.PRIUS: - dbc_f = 'toyota_prius_2017_pt_generated.dbc' signals += [("STATE", "AUTOPARK_STATUS", 0)] - elif CP.carFingerprint == CAR.RAV4H: - dbc_f = 'toyota_rav4_hybrid_2017_pt_generated.dbc' - elif CP.carFingerprint == CAR.RAV4: - dbc_f = 'toyota_rav4_2017_pt_generated.dbc' - elif CP.carFingerprint == CAR.COROLLA: - dbc_f = 'toyota_corolla_2017_pt_generated.dbc' - elif CP.carFingerprint == CAR.LEXUS_RXH: - dbc_f = 'lexus_rx_hybrid_2017_pt_generated.dbc' - - return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0) + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) class CarState(object): diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 0d6cf6d26f..23da3d4e23 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -5,8 +5,7 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.toyota.carstate import CarState, get_can_parser -from selfdrive.car.toyota.values import ECU, check_ecu_msgs -from common.fingerprints import TOYOTA as CAR +from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR try: from selfdrive.car.toyota.carcontroller import CarController @@ -20,9 +19,9 @@ class CarInterface(object): self.VM = VehicleModel(CP) self.frame = 0 - self.can_invalid_count = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False + self.can_invalid_count = 0 self.cruise_enabled_prev = False # *** init the major players *** @@ -70,6 +69,8 @@ class CarInterface(object): tireStiffnessRear_civic = 90000 ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] + ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay + if candidate == CAR.PRIUS: ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 @@ -82,6 +83,9 @@ class CarInterface(object): f = 1.43353663 tireStiffnessFront_civic *= f tireStiffnessRear_civic *= f + + # Prius has a very bad actuator + ret.steerActuatorDelay = 0.25 elif candidate in [CAR.RAV4, CAR.RAV4H]: ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.65 diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index c06f8b809f..1568ff5fa8 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -22,7 +22,7 @@ def _create_radard_can_parser(): return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) class RadarInterface(object): - def __init__(self): + def __init__(self, CP): # radar self.pts = {} self.validCnt = {key: 0 for key in RADAR_MSGS} @@ -86,7 +86,7 @@ class RadarInterface(object): return ret if __name__ == "__main__": - RI = RadarInterface() + RI = RadarInterface(None) while 1: ret = RI.update() print(chr(27) + "[2J") diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 937e32209c..4749ea247c 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1,4 +1,12 @@ -from common.fingerprints import TOYOTA as CAR +from selfdrive.car import dbc_dict + +class CAR: + PRIUS = "TOYOTA PRIUS 2017" + RAV4H = "TOYOTA RAV4 2017 HYBRID" + RAV4 = "TOYOTA RAV4 2017" + COROLLA = "TOYOTA COROLLA 2017" + LEXUS_RXH = "LEXUS RX HYBRID 2017" + class ECU: CAM = 0 # camera @@ -59,3 +67,43 @@ def check_ecu_msgs(fingerprint, candidate, ecu): x[3] == 0)] return any(msg for msg in fingerprint if msg in ecu_msgs) + + +FINGERPRINTS = { + CAR.RAV4: [{ + 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + }], + CAR.RAV4H: [{ + 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 296L: 8, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 4, 581L: 5, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 713L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 3, 955L: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1184L: 8, 1185L: 8, 1186L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1197L: 8, 1198L: 8, 1199L: 8, 1212L: 8, 1227L: 8, 1228L: 8, 1232L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + }], + CAR.PRIUS: [{ + 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + }, + # Prius Prime + { + 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 824L: 2, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2,898L: 8, 900L: 6, 902L: 6, 905L: 8, 913L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 974L: 8, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1076L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1164L: 8, 1165L: 8, 1166L: 8, 1167L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + }, + # Taiwanese Prius Prime + { + 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 824L: 2, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 845L: 5, 863L: 8, 869L: 7, 870L: 7, 871L: 2,898L: 8, 900L: 6, 902L: 6, 905L: 8, 913L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 974L: 8, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1076L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1164L: 8, 1165L: 8, 1166L: 8, 1167L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1264L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 + }], + CAR.COROLLA: [{ + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }, + # Corolla LE 2017 + { + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8 + }], + CAR.LEXUS_RXH: [{ + 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8 + }], +} + + +DBC = { + CAR.RAV4H: dbc_dict('toyota_rav4_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'), + CAR.RAV4: dbc_dict('toyota_rav4_2017_pt_generated', 'toyota_prius_2017_adas'), + CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_prius_2017_adas'), + CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_prius_2017_adas'), + CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'), +} diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index aa2be407b7..26eb53448b 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.4.6-release" +#define COMMA_VERSION "0.4.7-release" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5a281bb074..bcad96b02c 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -10,7 +10,7 @@ from common.params import Params import selfdrive.messaging as messaging from selfdrive.config import Conversions as CV from selfdrive.services import service_list -from selfdrive.car import get_car +from selfdrive.car.car_helpers import get_car from selfdrive.controls.lib.planner import Planner from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \ get_events, \ @@ -101,7 +101,7 @@ def data_sample(CI, CC, thermal, calibration, health, poller, cal_status, overte return CS, events, cal_status, overtemp, free_space -def calc_plan(CS, events, PL, LaC, LoC, v_cruise_kph, awareness_status): +def calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, awareness_status): # plan runs always, independently of the state plan_packet = PL.update(CS, LaC, LoC, v_cruise_kph, awareness_status < -0.) plan = plan_packet.plan @@ -112,7 +112,7 @@ def calc_plan(CS, events, PL, LaC, LoC, v_cruise_kph, awareness_status): # disable if lead isn't close when system is active and brake is pressed to avoid # unexpected vehicle accelerations - if CS.brakePressed and plan.vTargetFuture >= STARTING_TARGET_SPEED: + if CS.brakePressed and plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) return plan, plan_ts @@ -202,7 +202,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, + awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle, passive): # Given the state, this function returns the actuators @@ -412,7 +412,7 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_ return CC -def controlsd_thread(gctx, rate=100): +def controlsd_thread(gctx=None, rate=100, default_bias=0.): # start the loop set_realtime_priority(3) @@ -485,12 +485,12 @@ def controlsd_thread(gctx, rate=100): rk = Ratekeeper(rate, print_delay_threshold=2./1000) # learned angle offset - angle_offset = 1.5 # Default model bias + angle_offset = default_bias calibration_params = params.get("CalibrationParams") if calibration_params: try: calibration_params = json.loads(calibration_params) - angle_offset = calibration_params["angle_offset"] + angle_offset = calibration_params["angle_offset2"] except (ValueError, KeyError): pass @@ -498,7 +498,7 @@ def controlsd_thread(gctx, rate=100): while 1: - prof.checkpoint("Ratekeeper", ignore=True) # rk is here + prof.checkpoint("Ratekeeper", ignore=True) # sample data and compute car events CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, cal, health, poller, cal_status, @@ -506,7 +506,7 @@ def controlsd_thread(gctx, rate=100): prof.checkpoint("Sample") # define plan - plan, plan_ts = calc_plan(CS, events, PL, LaC, LoC, v_cruise_kph, awareness_status) + plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, awareness_status) prof.checkpoint("Plan") if not passive: diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 2fb365518e..de4873ed96 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -130,6 +130,12 @@ class AlertManager(object): AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, None, None, 0., 0., .2), + "resumeRequired": Alert( + "STOPPED", + "Press Resume to Move", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, None, None, 0., 0., .2), + "debugAlert": Alert( "DEBUG ALERT", "", diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 32e7a6a45b..74ccca832f 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -8,16 +8,13 @@ from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog from cereal import car -# 100ms is a rule of thumb estimation of lag from image processing to actuator command -ACTUATORS_DELAY = 0.1 - _DT = 0.01 # 100Hz _DT_MPC = 0.05 # 20Hz -def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio): - states[0].x = v_ego * ACTUATORS_DELAY - states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * ACTUATORS_DELAY +def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): + states[0].x = v_ego * delay + states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay return states @@ -70,7 +67,7 @@ class LatControl(object): p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly)) # account for actuation delay - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.steerRatio) + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.CP.steerRatio, VM.CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 7e9ca6f826..00c34557d6 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -64,7 +64,7 @@ def radard_thread(gctx=None): live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller) PP = PathPlanner() - RI = RadarInterface() + RI = RadarInterface(CP) last_md_ts = 0 last_l100_ts = 0 diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py index 75a5ea698d..d7cbabe63a 100755 --- a/selfdrive/debug/can_printer.py +++ b/selfdrive/debug/can_printer.py @@ -8,7 +8,7 @@ import selfdrive.messaging as messaging from selfdrive.services import service_list -def can_printer(bus=0, max_msg=0x10000, addr="127.0.0.1"): +def can_printer(bus=0, max_msg=None, addr="127.0.0.1"): context = zmq.Context() logcan = messaging.sub_sock(context, service_list['can'].port, addr=addr) @@ -27,7 +27,7 @@ def can_printer(bus=0, max_msg=0x10000, addr="127.0.0.1"): dd = chr(27) + "[2J" dd += "%5.2f\n" % (sec_since_boot() - start) for k,v in sorted(zip(msgs.keys(), map(lambda x: x[-1].encode("hex"), msgs.values()))): - if k < max_msg: + if max_msg is None or k < max_msg: dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v) print dd lp = sec_since_boot() diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py index 97476cc61d..dd8eb4d808 100755 --- a/selfdrive/debug/get_fingerprint.py +++ b/selfdrive/debug/get_fingerprint.py @@ -19,8 +19,9 @@ msgs = {} while True: lc = messaging.recv_sock(logcan, True) for c in lc.can: - # read also msgs sent by EON on CAN bus 0x80 - if c.src%0x80 == 0: + # read also msgs sent by EON on CAN bus 0x80 and filter out the + # addr with more than 11 bits + if c.src%0x80 == 0 and c.address < 0x800: msgs[c.address] = len(c.dat) fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items())) diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 9c271731d6..a34449a595 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 4c95bc06f6..53372d8916 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -62,7 +62,8 @@ import subprocess import traceback from multiprocessing import Process -if os.path.exists(os.path.join(BASEDIR, "vpn")): +EON = os.path.exists("/EON") +if EON and os.path.exists(os.path.join(BASEDIR, "vpn")): print "installing vpn" os.system(os.path.join(BASEDIR, "vpn", "install.sh")) @@ -82,7 +83,6 @@ import selfdrive.crash as crash from selfdrive.loggerd.config import ROOT -EON = os.path.exists("/EON") # comment out anything you don't want to run managed_processes = { @@ -102,7 +102,7 @@ managed_processes = { "visiond": ("selfdrive/visiond", ["./visiond"]), "sensord": ("selfdrive/sensord", ["./sensord"]), "gpsd": ("selfdrive/sensord", ["./gpsd"]), - "orbd": ("selfdrive/orbd", ["./orbd"]), + "orbd": ("selfdrive/orbd", ["./orbd_wrapper.sh"]), "updated": "selfdrive.updated", #"gpsplanner": "selfdrive.controls.gps_plannerd", } @@ -294,7 +294,10 @@ def system(cmd): output=e.output[-1024:], returncode=e.returncode) +LEON = False def setup_eon_fan(): + global LEON + if not EON: return @@ -302,15 +305,20 @@ def setup_eon_fan(): from smbus2 import SMBus bus = SMBus(7, force=True) - bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts - bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable - bus.write_byte_data(0x21, 0x02, 0x2) # needed? - bus.write_byte_data(0x21, 0x04, 0x4) # manual override source + try: + bus.write_byte_data(0x21, 0x10, 0xf) # mask all interrupts + bus.write_byte_data(0x21, 0x03, 0x1) # set drive current and global interrupt disable + bus.write_byte_data(0x21, 0x02, 0x2) # needed? + bus.write_byte_data(0x21, 0x04, 0x4) # manual override source + except IOError: + print "LEON detected" + #os.system("echo 1 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg") + LEON = True bus.close() last_eon_fan_val = None def set_eon_fan(val): - global last_eon_fan_val + global LEON, last_eon_fan_val if not EON: return @@ -318,9 +326,13 @@ def set_eon_fan(val): from smbus2 import SMBus if last_eon_fan_val is None or last_eon_fan_val != val: bus = SMBus(7, force=True) - bus.write_byte_data(0x21, 0x04, 0x2) - bus.write_byte_data(0x21, 0x03, (val*2)+1) - bus.write_byte_data(0x21, 0x04, 0x4) + if LEON: + i = [0x1, 0x3 | 0, 0x3 | 0x08, 0x3 | 0x10][val] + bus.write_i2c_block_data(0x3d, 0, [i]) + else: + bus.write_byte_data(0x21, 0x04, 0x2) + bus.write_byte_data(0x21, 0x03, (val*2)+1) + bus.write_byte_data(0x21, 0x04, 0x4) bus.close() last_eon_fan_val = val @@ -632,6 +644,9 @@ def uninstall(): os.system("service call power 16 i32 0 s16 recovery i32 1") def main(): + # the flippening! + os.system('LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1') + if os.getenv("NOLOG") is not None: del managed_processes['loggerd'] del managed_processes['tombstoned'] diff --git a/selfdrive/orbd/dsp/freethedsp.c b/selfdrive/orbd/dsp/freethedsp.c index 55c20e341f..298f4fd831 100644 --- a/selfdrive/orbd/dsp/freethedsp.c +++ b/selfdrive/orbd/dsp/freethedsp.c @@ -85,8 +85,8 @@ int ioctl(int fd, unsigned long request, void *arg) { struct fastrpc_ioctl_init *init = (struct fastrpc_ioctl_init *)arg; // confirm patch is correct and do the patch - assert(memcmp((void*)(init->mem+PATCH_ADDR), PATCH_OLD, 4) == 0); - memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, 4); + assert(memcmp((void*)(init->mem+PATCH_ADDR), PATCH_OLD, PATCH_LEN) == 0); + memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, PATCH_LEN); // flush cache int ionfd = open("/dev/ion", O_RDONLY); @@ -100,8 +100,8 @@ int ioctl(int fd, unsigned long request, void *arg) { struct ion_flush_data flush_data; flush_data.handle = fd_data.handle; flush_data.vaddr = (void*)init->mem; - flush_data.offset = PATCH_ADDR; - flush_data.length = PATCH_LEN; + flush_data.offset = 0; + flush_data.length = init->memlen; ret = ioctl(ionfd, ION_IOC_CLEAN_INV_CACHES, &flush_data); assert(ret == 0); diff --git a/selfdrive/orbd/orbd.cc b/selfdrive/orbd/orbd.cc index b35a38c5a2..26c9ffec76 100644 --- a/selfdrive/orbd/orbd.cc +++ b/selfdrive/orbd/orbd.cc @@ -4,6 +4,7 @@ #include #include #include +#include #include "common/visionipc.h" #include "common/swaglog.h" @@ -32,10 +33,14 @@ static void set_do_exit(int sig) { int main(int argc, char *argv[]) { int err; + setpriority(PRIO_PROCESS, 0, -13); printf("starting orbd\n"); #ifdef DSP + uint32_t test_leet = 0; char my_path[PATH_MAX+1]; + memset(my_path, 0, sizeof(my_path)); + ssize_t len = readlink("/proc/self/exe", my_path, sizeof(my_path)); assert(len > 5); my_path[len-5] = '\0'; @@ -45,7 +50,6 @@ int main(int argc, char *argv[]) { snprintf(adsp_path, PATH_MAX, "ADSP_LIBRARY_PATH=%s/dsp/gen", my_path); assert(putenv(adsp_path) == 0); - uint32_t test_leet = 0; assert(calculator_init(&test_leet) == 0); assert(test_leet == 0x1337); LOGW("orbd init complete"); diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index 7513ac78bf..ba030423d2 100755 Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index a7a275e1ae..873d0055f5 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 022a576ea7..8ff062e02a 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -12,11 +12,11 @@ from selfdrive.config import Conversions as CV import selfdrive.messaging as messaging from selfdrive.services import service_list from selfdrive.car.honda.hondacan import fix -from common.fingerprints import HONDA as CAR +from selfdrive.car.honda.values import CAR from selfdrive.car.honda.carstate import get_can_signals from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp -from selfdrive.car.honda.old_can_parser import CANParser +from selfdrive.can.plant_can_parser import CANParser from selfdrive.car.honda.interface import CarInterface from common.dbc import dbc @@ -145,7 +145,7 @@ class Plant(object): return float(self.rk.frame) / self.rate def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True): - gen_dbc, gen_signals, gen_checks = get_can_signals(CP) + gen_signals, gen_checks = get_can_signals(CP) sgs = [s[0] for s in gen_signals] msgs = [s[1] for s in gen_signals] cks_msgs = set(check[0] for check in gen_checks) @@ -213,13 +213,10 @@ class Plant(object): # TODO: the order is this list should not matter, but currently everytime we change carstate we break this test. Fix it! vls = [self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.angle_steer, self.angle_steer_rate, 0, - 0, 0, 0, 0, # Doors 0, 0, # Blinkers - 0, # Cruise speed offset self.gear_choice, speed != 0, self.brake_error, self.brake_error, - self.v_cruise, not self.seatbelt, self.seatbelt, # Seatbelt self.brake_pressed, 0., cruise_buttons, @@ -231,12 +228,20 @@ class Plant(object): self.pedal_gas, self.cruise_setting, self.acc_status, + + self.v_cruise, + 0, # Cruise speed offset + + 0, 0, 0, 0, # Doors + self.user_gas, self.main_on, 0, # EPB State 0, # Brake hold 0, # Interceptor feedback # 0, + + ] # TODO: publish each message at proper frequency diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index daac5a1c86..17a31f493a 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -222,8 +222,8 @@ typedef struct UIState { } UIState; static int last_brightness = -1; -static void set_brightness(int brightness) { - if (last_brightness != brightness) { +static void set_brightness(UIState *s, int brightness) { + if (last_brightness != brightness && (s->awake || brightness == 0)) { FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb"); if (f != NULL) { fprintf(f, "%d", brightness); @@ -246,6 +246,7 @@ static void set_awake(UIState *s, bool awake) { framebuffer_set_power(s->fb, HWC_POWER_MODE_NORMAL); } else { LOG("awake off"); + set_brightness(s, 0); framebuffer_set_power(s->fb, HWC_POWER_MODE_OFF); } } @@ -454,7 +455,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, s->cur_vision_front_idx = -1; s->scene = (UIScene){ - .frontview = 0, + .frontview = getenv("FRONTVIEW") != NULL, .cal_status = CALIBRATION_CALIBRATED, .transformed_width = ui_info.transformed_width, .transformed_height = ui_info.transformed_height, @@ -1587,11 +1588,10 @@ static void* vision_connect_thread(void *args) { return NULL; } + #include #include -#define SENSOR_LIGHT 7 - static void* light_sensor_thread(void *args) { int err; @@ -1608,6 +1608,14 @@ static void* light_sensor_thread(void *args) { struct sensor_t const* list; int count = module->get_sensors_list(module, &list); + int SENSOR_LIGHT = 7; + + struct stat buffer; + if (stat("/sys/bus/i2c/drivers/cyccg", &buffer) == 0) { + LOGD("LeEco light sensor detected"); + SENSOR_LIGHT = 5; + } + device->activate(device, SENSOR_LIGHT, 0); device->activate(device, SENSOR_LIGHT, 1); device->setDelay(device, SENSOR_LIGHT, ms2ns(100)); @@ -1621,7 +1629,8 @@ static void* light_sensor_thread(void *args) { LOG_100("light_sensor_poll failed: %d", n); } if (n > 0) { - s->light_sensor = buffer[0].light; + if (SENSOR_LIGHT == 5) s->light_sensor = buffer[0].light * 2; + else s->light_sensor = buffer[0].light; //printf("new light sensor value: %f\n", s->light_sensor); } } @@ -1715,10 +1724,10 @@ int main() { float clipped_light_sensor = (s->light_sensor*LIGHT_SENSOR_M) + LIGHT_SENSOR_B; if (clipped_light_sensor > 255) clipped_light_sensor = 255; smooth_light_sensor = clipped_light_sensor * 0.01 + smooth_light_sensor * 0.99; - set_brightness((int)smooth_light_sensor); + set_brightness(s, (int)smooth_light_sensor); } else { // compromise for bright and dark envs - set_brightness(NEO_BRIGHTNESS); + set_brightness(s, NEO_BRIGHTNESS); } ui_update(s); diff --git a/selfdrive/version.py b/selfdrive/version.py index 97d960678c..8166c662f4 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -4,8 +4,8 @@ with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "ve version = _versionf.read().split('"')[1] try: - origin = subprocess.check_output(["git", "config", "--get", "remote.origin.url"]) - if origin.startswith('git@github.com:commaai'): + origin = subprocess.check_output(["git", "config", "--get", "remote.origin.url"]).rstrip() + if origin.startswith('git@github.com:commaai') or origin.startswith('https://github.com/commaai'): if origin.endswith('/one.git'): dirty = True else: @@ -19,4 +19,3 @@ except subprocess.CalledProcessError: # put this here training_version = "0.1.0" - diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 65bf902d0b..2fdec6e53f 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ