openpilot v0.4.7 release
Vehicle Researcher
parent
e3ab9d6460
commit
ae5cb7a0da
|
|
@ -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
|
||||
|
|
|
|||
76
README.md
76
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
|
||||
------
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
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
|
||||
|
||||
|
||||
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"
|
||||
_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]
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@ struct Signal {
|
|||
int b1, b2, bo;
|
||||
bool is_signed;
|
||||
double factor, offset;
|
||||
bool is_little_endian;
|
||||
SignalType type;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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" %}
|
||||
|
|
|
|||
|
|
@ -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<cereal::CanData>::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);
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,4 @@
|
|||
### old can parser just used by plant.py for regression tests
|
||||
import os
|
||||
import opendbc
|
||||
from collections import defaultdict
|
||||
|
|
@ -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/<name>/
|
||||
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}
|
||||
|
|
|
|||
|
|
@ -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/<name>/
|
||||
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
|
||||
|
|
@ -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"])
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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'),
|
||||
}
|
||||
|
|
@ -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())
|
||||
|
|
@ -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
|
||||
|
||||
|
|
@ -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]
|
||||
|
||||
'''
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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'),
|
||||
}
|
||||
|
|
@ -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))
|
||||
|
||||
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))
|
||||
# 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))
|
||||
|
||||
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
|
||||
|
|
|
|||
|
|
@ -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']
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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]))
|
||||
|
|
|
|||
|
|
@ -1,12 +1,10 @@
|
|||
#!/usr/bin/env python
|
||||
import os
|
||||
|
||||
from selfdrive.can.parser import CANParser
|
||||
|
||||
from cereal import car
|
||||
from common.realtime import sec_since_boot
|
||||
|
||||
import zmq
|
||||
import time
|
||||
from cereal import car
|
||||
from selfdrive.can.parser import CANParser
|
||||
from common.realtime import sec_since_boot
|
||||
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")
|
||||
|
|
|
|||
|
|
@ -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'),
|
||||
}
|
||||
|
|
|
|||
|
|
@ -71,6 +71,7 @@ class CarInterface(object):
|
|||
ret.longitudinalKpV = [0.]
|
||||
ret.longitudinalKiBP = [0.]
|
||||
ret.longitudinalKiV = [0.]
|
||||
ret.steerActuatorDelay = 0.
|
||||
|
||||
return ret
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,2 @@
|
|||
class CAR:
|
||||
MOCK = 'mock'
|
||||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
|
|
|
|||
|
|
@ -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'),
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
#define COMMA_VERSION "0.4.6-release"
|
||||
#define COMMA_VERSION "0.4.7-release"
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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",
|
||||
"",
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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()))
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -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']
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@
|
|||
#include <unistd.h>
|
||||
#include <stdint.h>
|
||||
#include <assert.h>
|
||||
#include <sys/resource.h>
|
||||
|
||||
#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");
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <hardware/sensors.h>
|
||||
#include <utils/Timers.h>
|
||||
|
||||
#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);
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
||||
|
|
|
|||
Binary file not shown.
Loading…
Reference in New Issue