mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-12 10:24:14 +08:00
Compare commits
12 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
8f3539a27b | ||
|
|
e8ae37e36e | ||
|
|
d0715b6562 | ||
|
|
a2c76acf3b | ||
|
|
a51a60b598 | ||
|
|
7ed5c6554d | ||
|
|
5641fc986d | ||
|
|
c499aa549c | ||
|
|
0a99fe3baa | ||
|
|
8cc32df779 | ||
|
|
8291f35701 | ||
|
|
0f885c87a5 |
105
README.md
105
README.md
@@ -1,9 +1,9 @@
|
||||
[](#)
|
||||
[](#)
|
||||
|
||||
Welcome to openpilot
|
||||
======
|
||||
|
||||
[openpilot](http://github.com/commaai/openpilot) is an open source driving agent. Currently it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for Hondas, Acuras, Toyotas, and a Chevy. It's about on par with Tesla Autopilot and GM Super Cruise, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
|
||||
[openpilot](http://github.com/commaai/openpilot) is an open source driving agent. Currently it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for selected Honda, Toyota, Acura, Lexus, Chevrolet, Hyundai, Kia. It's about on par with Tesla Autopilot and GM Super Cruise, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
|
||||
|
||||
The openpilot codebase has been written to be concise and enable rapid prototyping. We look forward to your contributions - improving real vehicle automation has never been easier.
|
||||
|
||||
@@ -14,7 +14,7 @@ openpilot is developed by [comma.ai](https://comma.ai/) and users like you.
|
||||
|
||||
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).
|
||||
Also, we have a several thousand people community on [slack](https://slack.comma.ai).
|
||||
|
||||
<table>
|
||||
<tr>
|
||||
@@ -41,54 +41,54 @@ Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` dur
|
||||
Supported Cars
|
||||
------
|
||||
|
||||
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below |
|
||||
| -------------------| ----------------------| ---------------------| --------| ---------------| -----------------| ---------------|
|
||||
| Acura | ILX 2016 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| GM<sup>3</sup> | Volt 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| GM<sup>3</sup> | Volt 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph |
|
||||
| Honda | Civic 2016 | Honda Sensing | Yes | Yes | 0mph | 12mph |
|
||||
| Honda | Civic 2017 | Honda Sensing | Yes | Yes | 0mph | 12mph |
|
||||
| Honda | Civic 2017 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | Civic 2018 | Honda Sensing | Yes | Yes | 0mph | 12mph |
|
||||
| Honda | Civic 2018 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V 2015 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | CR-V 2016 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | CR-V 2017 | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V 2018 | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | Odyssey 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Odyssey 2019 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Hyundai<sup>6</sup>| Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Hyundai<sup>6</sup>| Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph |
|
||||
| Hyundai<sup>6</sup>| Genesis 2018 | All | Yes | Stock | 19mph | 34mph |
|
||||
| Kia<sup>6</sup> | Sorento 2018 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Kia<sup>6</sup> | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Camry 2018<sup>4</sup>| All | Yes | Stock | 0mph<sup>5</sup> | 0mph |
|
||||
| Toyota | C-HR 2018<sup>4</sup> | All | Yes | Stock | 0mph | 0mph |
|
||||
| Toyota | Corolla 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Corolla 2018 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Highlander 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander Hybrid 2018| All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 2018 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below |
|
||||
| ---------------------| ----------------------| ---------------------| --------| ---------------| -----------------| ---------------|
|
||||
| Acura | ILX 2016 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | ILX 2017 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Chevrolet<sup>3</sup>| Volt 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Chevrolet<sup>3</sup>| Volt 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph |
|
||||
| Honda | Civic 2016 | Honda Sensing | Yes | Yes | 0mph | 12mph |
|
||||
| Honda | Civic 2017 | Honda Sensing | Yes | Yes | 0mph | 12mph |
|
||||
| Honda | Civic 2017 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | Civic 2018 | Honda Sensing | Yes | Yes | 0mph | 12mph |
|
||||
| Honda | Civic 2018 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V 2015 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | CR-V 2016 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | CR-V 2017 | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V 2018 | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | Odyssey 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Odyssey 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Odyssey 2019 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Pilot 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2017 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Hyundai<sup>6</sup> | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Hyundai<sup>6</sup> | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph |
|
||||
| Hyundai<sup>6</sup> | Genesis 2018 | All | Yes | Stock | 19mph | 34mph |
|
||||
| Kia<sup>6</sup> | Sorento 2018 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Kia<sup>6</sup> | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Camry 2018<sup>4</sup>| All | Yes | Stock | 0mph<sup>5</sup> | 0mph |
|
||||
| Toyota | C-HR 2018<sup>4</sup> | All | Yes | Stock | 0mph | 0mph |
|
||||
| Toyota | Corolla 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Corolla 2018 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Highlander 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander Hybrid 2018| All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 2017 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 2018 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2017 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
|
||||
<sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)***
|
||||
<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota)
|
||||
@@ -103,8 +103,11 @@ Community Maintained Cars
|
||||
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below |
|
||||
| ------- | ---------------------- | -------------------- | ------- | ------------ | -------------- | -------------- |
|
||||
| Honda | Fit 2018 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Tesla | Model S 2012 | All | Yes | Not yet | Not applicable | 0mph |
|
||||
| Tesla | Model S 2013 | All | Yes | Not yet | Not applicable | 0mph |
|
||||
|
||||
[[Honda Fit Pull Request]](https://github.com/commaai/openpilot/pull/266).
|
||||
[[Tesla Model S Pull Request]](https://github.com/commaai/openpilot/pull/246)
|
||||
|
||||
Community Maintained Cars are not confirmed by comma.ai to meet our [safety model](https://github.com/commaai/openpilot/blob/devel/SAFETY.md). Be extra cautious using them.
|
||||
|
||||
|
||||
@@ -1,3 +1,9 @@
|
||||
Version 0.5.5 (2018-10-20)
|
||||
========================
|
||||
* Increase allowed Honda positive accelerations
|
||||
* Fix sporadic unexpected braking when passing semi-trucks in Toyota
|
||||
* Fix gear reading bug in Hyundai Elantra thanks to emmertex!
|
||||
|
||||
Version 0.5.4 (2018-09-25)
|
||||
========================
|
||||
* New Driving Model
|
||||
|
||||
@@ -410,6 +410,7 @@ struct Live100Data {
|
||||
alertStatus @38 :AlertStatus;
|
||||
alertSize @39 :AlertSize;
|
||||
alertBlinkingRate @42 :Float32;
|
||||
alertType @44 :Text;
|
||||
awarenessStatus @26 :Float32;
|
||||
angleOffset @27 :Float32;
|
||||
gpsPlannerActive @40 :Bool;
|
||||
@@ -1558,6 +1559,13 @@ struct Boot {
|
||||
lastPmsg @2 :Data;
|
||||
}
|
||||
|
||||
struct LiveParametersData {
|
||||
valid @0 :Bool;
|
||||
gyroBias @1 :Float32;
|
||||
angleOffset @2 :Float32;
|
||||
}
|
||||
|
||||
|
||||
struct Event {
|
||||
# in nanoseconds?
|
||||
logMonoTime @0 :UInt64;
|
||||
@@ -1623,5 +1631,6 @@ struct Event {
|
||||
orbFeaturesSummary @58 :OrbFeaturesSummary;
|
||||
driverMonitoring @59 :DriverMonitoring;
|
||||
boot @60 :Boot;
|
||||
liveParameters @61 :LiveParametersData;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,7 +6,7 @@ from cffi import FFI
|
||||
|
||||
TMPDIR = "/tmp/ccache"
|
||||
|
||||
def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR):
|
||||
def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=[]):
|
||||
cache = name + "_" + hashlib.sha1(c_code).hexdigest()
|
||||
try:
|
||||
os.mkdir(tmpdir)
|
||||
@@ -21,19 +21,19 @@ def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR):
|
||||
mod = __import__(cache)
|
||||
except Exception:
|
||||
print "cache miss", cache
|
||||
compile_code(cache, c_code, c_header, tmpdir)
|
||||
compile_code(cache, c_code, c_header, tmpdir, cflags, libraries)
|
||||
mod = __import__(cache)
|
||||
finally:
|
||||
os.close(fd)
|
||||
|
||||
return mod.ffi, mod.lib
|
||||
|
||||
def compile_code(name, c_code, c_header, directory):
|
||||
def compile_code(name, c_code, c_header, directory, cflags="", libraries=[]):
|
||||
ffibuilder = FFI()
|
||||
ffibuilder.set_source(name, c_code, source_extension='.cpp')
|
||||
ffibuilder.set_source(name, c_code, source_extension='.cpp', libraries=libraries)
|
||||
ffibuilder.cdef(c_header)
|
||||
os.environ['OPT'] = "-fwrapv -O2 -DNDEBUG -std=c++11"
|
||||
os.environ['CFLAGS'] = ""
|
||||
os.environ['CFLAGS'] = cflags
|
||||
ffibuilder.compile(verbose=True, debug=False, tmpdir=directory)
|
||||
|
||||
def wrap_compiled(name, directory):
|
||||
|
||||
@@ -75,10 +75,8 @@ def get_model_height_transform(camera_frame_from_road_frame, height):
|
||||
[0, 0, 1],
|
||||
]))
|
||||
|
||||
ground_from_camera_frame = np.linalg.inv(camera_frame_from_road_ground)
|
||||
|
||||
low_camera_from_high_camera = np.dot(camera_frame_from_road_high, ground_from_camera_frame)
|
||||
high_camera_from_low_camera = np.linalg.inv(low_camera_from_high_camera)
|
||||
road_high_from_camera_frame = np.linalg.inv(camera_frame_from_road_high)
|
||||
high_camera_from_low_camera = np.dot(camera_frame_from_road_ground, road_high_from_camera_frame)
|
||||
|
||||
return high_camera_from_low_camera
|
||||
|
||||
|
||||
@@ -182,80 +182,95 @@ BO_ 543 TRACK_A_15: 8 XXX
|
||||
BO_ 544 TRACK_B_0: 8 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 545 TRACK_B_1: 8 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 546 TRACK_B_2: 8 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 547 TRACK_B_3: 8 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 548 TRACK_B_4: 8 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 549 TRACK_B_5: 8 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 550 TRACK_B_6: 8 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 551 TRACK_B_7: 8 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 552 TRACK_B_8: 6 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 553 TRACK_B_9: 6 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 554 TRACK_B_10: 6 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 555 TRACK_B_11: 6 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 556 TRACK_B_12: 6 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 557 TRACK_B_13: 6 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 558 TRACK_B_14: 6 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
BO_ 559 TRACK_B_15: 6 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ REL_ACCEL : 15|7@0- (1,0) [-64|63] "" XXX
|
||||
SG_ SCORE : 23|8@0+ (1,0) [0|100] "" XXX
|
||||
SG_ CHECKSUM : 47|8@0+ (1,0) [0|255] "" XXX
|
||||
|
||||
|
||||
@@ -1 +1 @@
|
||||
v1.1.5
|
||||
v1.1.6
|
||||
@@ -56,6 +56,7 @@ int controls_allowed = 0;
|
||||
#include "safety/safety_toyota.h"
|
||||
#ifdef PANDA
|
||||
#include "safety/safety_toyota_ipas.h"
|
||||
#include "safety/safety_tesla.h"
|
||||
#endif
|
||||
#include "safety/safety_gm.h"
|
||||
#include "safety/safety_ford.h"
|
||||
@@ -100,6 +101,7 @@ typedef struct {
|
||||
#define SAFETY_FORD 5
|
||||
#define SAFETY_CADILLAC 6
|
||||
#define SAFETY_HYUNDAI 7
|
||||
#define SAFETY_TESLA 8
|
||||
#define SAFETY_TOYOTA_IPAS 0x1335
|
||||
#define SAFETY_TOYOTA_NOLIMITS 0x1336
|
||||
#define SAFETY_ALLOUTPUT 0x1337
|
||||
@@ -117,6 +119,7 @@ const safety_hook_config safety_hook_registry[] = {
|
||||
{SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
|
||||
#ifdef PANDA
|
||||
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
|
||||
{SAFETY_TESLA, &tesla_hooks},
|
||||
#endif
|
||||
{SAFETY_ALLOUTPUT, &alloutput_hooks},
|
||||
{SAFETY_ELM327, &elm327_hooks},
|
||||
|
||||
287
panda/board/safety/safety_tesla.h
Normal file
287
panda/board/safety/safety_tesla.h
Normal file
@@ -0,0 +1,287 @@
|
||||
// board enforces
|
||||
// in-state
|
||||
// accel set/resume
|
||||
// out-state
|
||||
// cancel button
|
||||
// regen paddle
|
||||
// accel rising edge
|
||||
// brake rising edge
|
||||
// brake > 0mph
|
||||
//
|
||||
int fmax_limit_check(float val, const float MAX, const float MIN) {
|
||||
return (val > MAX) || (val < MIN);
|
||||
}
|
||||
|
||||
// 2m/s are added to be less restrictive
|
||||
const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_UP = {
|
||||
{2., 7., 17.},
|
||||
{5., .8, .25}};
|
||||
|
||||
const struct lookup_t TESLA_LOOKUP_ANGLE_RATE_DOWN = {
|
||||
{2., 7., 17.},
|
||||
{5., 3.5, .8}};
|
||||
|
||||
const struct lookup_t TESLA_LOOKUP_MAX_ANGLE = {
|
||||
{2., 29., 38.},
|
||||
{410., 92., 36.}};
|
||||
|
||||
const int TESLA_RT_INTERVAL = 250000; // 250ms between real time checks
|
||||
|
||||
// state of angle limits
|
||||
float tesla_desired_angle_last = 0; // last desired steer angle
|
||||
float tesla_rt_angle_last = 0.; // last real time angle
|
||||
float tesla_ts_angle_last = 0;
|
||||
|
||||
int tesla_controls_allowed_last = 0;
|
||||
|
||||
int tesla_brake_prev = 0;
|
||||
int tesla_gas_prev = 0;
|
||||
int tesla_speed = 0;
|
||||
int eac_status = 0;
|
||||
|
||||
int tesla_ignition_started = 0;
|
||||
|
||||
|
||||
void set_gmlan_digital_output(int to_set);
|
||||
void reset_gmlan_switch_timeout(void);
|
||||
void gmlan_switch_init(int timeout_enable);
|
||||
|
||||
|
||||
static void tesla_rx_hook(CAN_FIFOMailBox_TypeDef *to_push)
|
||||
{
|
||||
set_gmlan_digital_output(0); // #define GMLAN_HIGH 0
|
||||
reset_gmlan_switch_timeout(); //we're still in tesla safety mode, reset the timeout counter and make sure our output is enabled
|
||||
|
||||
//int bus_number = (to_push->RDTR >> 4) & 0xFF;
|
||||
uint32_t addr;
|
||||
if (to_push->RIR & 4)
|
||||
{
|
||||
// Extended
|
||||
// Not looked at, but have to be separated
|
||||
// to avoid address collision
|
||||
addr = to_push->RIR >> 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Normal
|
||||
addr = to_push->RIR >> 21;
|
||||
}
|
||||
|
||||
if (addr == 0x45)
|
||||
{
|
||||
// 6 bits starting at position 0
|
||||
int lever_position = (to_push->RDLR & 0x3F);
|
||||
if (lever_position == 2)
|
||||
{ // pull forward
|
||||
// activate openpilot
|
||||
controls_allowed = 1;
|
||||
//}
|
||||
}
|
||||
else if (lever_position == 1)
|
||||
{ // push towards the back
|
||||
// deactivate openpilot
|
||||
controls_allowed = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Detect drive rail on (ignition) (start recording)
|
||||
if (addr == 0x348)
|
||||
{
|
||||
// GTW_status
|
||||
int drive_rail_on = (to_push->RDLR & 0x0001);
|
||||
tesla_ignition_started = drive_rail_on == 1;
|
||||
}
|
||||
|
||||
// exit controls on brake press
|
||||
// DI_torque2::DI_brakePedal 0x118
|
||||
if (addr == 0x118)
|
||||
{
|
||||
// 1 bit at position 16
|
||||
if (((to_push->RDLR & 0x8000)) >> 15 == 1)
|
||||
{
|
||||
//disable break cancel by commenting line below
|
||||
controls_allowed = 0;
|
||||
}
|
||||
//get vehicle speed in m/s. Tesla gives MPH
|
||||
tesla_speed = ((((((to_push->RDLR >> 24) & 0x0F) << 8) + ((to_push->RDLR >> 16) & 0xFF)) * 0.05 - 25) * 1.609 / 3.6);
|
||||
if (tesla_speed < 0)
|
||||
{
|
||||
tesla_speed = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// exit controls on EPAS error
|
||||
// EPAS_sysStatus::EPAS_eacStatus 0x370
|
||||
if (addr == 0x370)
|
||||
{
|
||||
// if EPAS_eacStatus is not 1 or 2, disable control
|
||||
eac_status = ((to_push->RDHR >> 21)) & 0x7;
|
||||
// For human steering override we must not disable controls when eac_status == 0
|
||||
// Additional safety: we could only allow eac_status == 0 when we have human steering allowed
|
||||
if ((controls_allowed == 1) && (eac_status != 0) && (eac_status != 1) && (eac_status != 2))
|
||||
{
|
||||
controls_allowed = 0;
|
||||
//puts("EPAS error! \n");
|
||||
}
|
||||
}
|
||||
//get latest steering wheel angle
|
||||
if (addr == 0x00E)
|
||||
{
|
||||
float angle_meas_now = (int)((((to_push->RDLR & 0x3F) << 8) + ((to_push->RDLR >> 8) & 0xFF)) * 0.1 - 819.2);
|
||||
uint32_t ts = TIM2->CNT;
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, tesla_ts_angle_last);
|
||||
|
||||
// *** angle real time check
|
||||
// add 1 to not false trigger the violation and multiply by 25 since the check is done every 250 ms and steer angle is updated at 100Hz
|
||||
float rt_delta_angle_up = interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) * 25. + 1.;
|
||||
float rt_delta_angle_down = interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) * 25. + 1.;
|
||||
float highest_rt_angle = tesla_rt_angle_last + (tesla_rt_angle_last > 0 ? rt_delta_angle_up : rt_delta_angle_down);
|
||||
float lowest_rt_angle = tesla_rt_angle_last - (tesla_rt_angle_last > 0 ? rt_delta_angle_down : rt_delta_angle_up);
|
||||
|
||||
if ((ts_elapsed > TESLA_RT_INTERVAL) || (controls_allowed && !tesla_controls_allowed_last))
|
||||
{
|
||||
tesla_rt_angle_last = angle_meas_now;
|
||||
tesla_ts_angle_last = ts;
|
||||
}
|
||||
|
||||
// check for violation;
|
||||
if (fmax_limit_check(angle_meas_now, highest_rt_angle, lowest_rt_angle))
|
||||
{
|
||||
// We should not be able to STEER under these conditions
|
||||
// Other sending is fine (to allow human override)
|
||||
controls_allowed = 0;
|
||||
//puts("WARN: RT Angle - No steer allowed! \n");
|
||||
}
|
||||
else
|
||||
{
|
||||
controls_allowed = 1;
|
||||
}
|
||||
|
||||
tesla_controls_allowed_last = controls_allowed;
|
||||
}
|
||||
}
|
||||
|
||||
// all commands: gas/regen, friction brake and steering
|
||||
// if controls_allowed and no pedals pressed
|
||||
// allow all commands up to limit
|
||||
// else
|
||||
// block all commands that produce actuation
|
||||
|
||||
static int tesla_tx_hook(CAN_FIFOMailBox_TypeDef *to_send)
|
||||
{
|
||||
|
||||
uint32_t addr;
|
||||
float angle_raw;
|
||||
float desired_angle;
|
||||
|
||||
addr = to_send->RIR >> 21;
|
||||
|
||||
// do not transmit CAN message if steering angle too high
|
||||
// DAS_steeringControl::DAS_steeringAngleRequest
|
||||
if (addr == 0x488)
|
||||
{
|
||||
angle_raw = ((to_send->RDLR & 0x7F) << 8) + ((to_send->RDLR & 0xFF00) >> 8);
|
||||
desired_angle = angle_raw * 0.1 - 1638.35;
|
||||
int16_t violation = 0;
|
||||
int st_enabled = (to_send->RDLR & 0x400000) >> 22;
|
||||
|
||||
if (st_enabled == 0) {
|
||||
//steering is not enabled, do not check angles and do send
|
||||
tesla_desired_angle_last = desired_angle;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (controls_allowed)
|
||||
{
|
||||
// add 1 to not false trigger the violation
|
||||
float delta_angle_up = interpolate(TESLA_LOOKUP_ANGLE_RATE_UP, tesla_speed) + 1.;
|
||||
float delta_angle_down = interpolate(TESLA_LOOKUP_ANGLE_RATE_DOWN, tesla_speed) + 1.;
|
||||
float highest_desired_angle = tesla_desired_angle_last + (tesla_desired_angle_last > 0 ? delta_angle_up : delta_angle_down);
|
||||
float lowest_desired_angle = tesla_desired_angle_last - (tesla_desired_angle_last > 0 ? delta_angle_down : delta_angle_up);
|
||||
float TESLA_MAX_ANGLE = interpolate(TESLA_LOOKUP_MAX_ANGLE, tesla_speed) + 1.;
|
||||
|
||||
//check for max angles
|
||||
violation |= fmax_limit_check(desired_angle, TESLA_MAX_ANGLE, -TESLA_MAX_ANGLE);
|
||||
|
||||
//check for angle delta changes
|
||||
violation |= fmax_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||
|
||||
if (violation)
|
||||
{
|
||||
controls_allowed = 0;
|
||||
return false;
|
||||
}
|
||||
tesla_desired_angle_last = desired_angle;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static int tesla_tx_lin_hook(int lin_num, uint8_t *data, int len)
|
||||
{
|
||||
// LIN is not used on the Tesla
|
||||
return false;
|
||||
}
|
||||
|
||||
static void tesla_init(int16_t param)
|
||||
{
|
||||
controls_allowed = 0;
|
||||
tesla_ignition_started = 0;
|
||||
gmlan_switch_init(1); //init the gmlan switch with 1s timeout enabled
|
||||
}
|
||||
|
||||
static int tesla_ign_hook()
|
||||
{
|
||||
return tesla_ignition_started;
|
||||
}
|
||||
|
||||
static int tesla_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd)
|
||||
{
|
||||
|
||||
int32_t addr = to_fwd->RIR >> 21;
|
||||
|
||||
if (bus_num == 0)
|
||||
{
|
||||
|
||||
// change inhibit of GTW_epasControl
|
||||
if (addr == 0x101)
|
||||
{
|
||||
to_fwd->RDLR = to_fwd->RDLR | 0x4000; // 0x4000: WITH_ANGLE, 0xC000: WITH_BOTH (angle and torque)
|
||||
int checksum = (((to_fwd->RDLR & 0xFF00) >> 8) + (to_fwd->RDLR & 0xFF) + 2) & 0xFF;
|
||||
to_fwd->RDLR = to_fwd->RDLR & 0xFFFF;
|
||||
to_fwd->RDLR = to_fwd->RDLR + (checksum << 16);
|
||||
return 2;
|
||||
}
|
||||
|
||||
// remove EPB_epasControl
|
||||
if (addr == 0x214)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return 2; // Custom EPAS bus
|
||||
}
|
||||
if (bus_num == 2)
|
||||
{
|
||||
|
||||
// remove GTW_epasControl in forwards
|
||||
if (addr == 0x101)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return 0; // Chassis CAN
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
const safety_hooks tesla_hooks = {
|
||||
.init = tesla_init,
|
||||
.rx = tesla_rx_hook,
|
||||
.tx = tesla_tx_hook,
|
||||
.tx_lin = tesla_tx_lin_hook,
|
||||
.ignition = tesla_ign_hook,
|
||||
.fwd = tesla_fwd_hook,
|
||||
};
|
||||
@@ -183,3 +183,13 @@ void init_tests_honda(void){
|
||||
brake_prev = 0;
|
||||
gas_prev = 0;
|
||||
}
|
||||
|
||||
|
||||
void set_gmlan_digital_output(int to_set){
|
||||
}
|
||||
|
||||
void reset_gmlan_switch_timeout(void){
|
||||
}
|
||||
|
||||
void gmlan_switch_init(int timeout_enable){
|
||||
}
|
||||
|
||||
0
selfdrive/assets/sounds/Icon
Normal file
0
selfdrive/assets/sounds/Icon
Normal file
BIN
selfdrive/assets/sounds/hq_disengaged.mp3
Normal file
BIN
selfdrive/assets/sounds/hq_disengaged.mp3
Normal file
Binary file not shown.
BIN
selfdrive/assets/sounds/hq_engaged.mp3
Normal file
BIN
selfdrive/assets/sounds/hq_engaged.mp3
Normal file
Binary file not shown.
BIN
selfdrive/assets/sounds/hq_warning_0.mp3
Normal file
BIN
selfdrive/assets/sounds/hq_warning_0.mp3
Normal file
Binary file not shown.
BIN
selfdrive/assets/sounds/hq_warning_1.mp3
Normal file
BIN
selfdrive/assets/sounds/hq_warning_1.mp3
Normal file
Binary file not shown.
BIN
selfdrive/assets/sounds/hq_warning_2.mp3
Normal file
BIN
selfdrive/assets/sounds/hq_warning_2.mp3
Normal file
Binary file not shown.
@@ -4,7 +4,7 @@ from selfdrive.config import Conversions as CV
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.gm import gmcan
|
||||
from selfdrive.car.gm.values import CAR, DBC, AccState
|
||||
from selfdrive.car.gm.values import CAR, DBC
|
||||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
|
||||
@@ -26,14 +26,18 @@ class CarControllerParams():
|
||||
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
|
||||
# Takes case of "Service Adaptive Cruise" and "Service Front Camera"
|
||||
# dashboard messages.
|
||||
self.ADAS_KEEPALIVE_STEP = 100
|
||||
self.CAMERA_KEEPALIVE_STEP = 100
|
||||
|
||||
# pedal lookups, only for Volt
|
||||
MAX_GAS = 3072 # Only a safety limit
|
||||
self.ZERO_GAS = 2048
|
||||
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, self.ZERO_GAS, MAX_GAS]
|
||||
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]
|
||||
|
||||
@@ -59,12 +63,11 @@ class CarController(object):
|
||||
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
|
||||
self.allow_controls = allow_controls
|
||||
self.lka_icon_status_last = (False, False)
|
||||
|
||||
# Setup detection helper. Routes commands to
|
||||
# an appropriate CAN bus number.
|
||||
@@ -83,6 +86,7 @@ class CarController(object):
|
||||
return
|
||||
|
||||
P = self.params
|
||||
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
canbus = self.canbus
|
||||
@@ -130,18 +134,12 @@ class CarController(object):
|
||||
if (frame % 4) == 0:
|
||||
idx = (frame / 4) % 4
|
||||
|
||||
car_stopping = apply_gas < P.ZERO_GAS
|
||||
standstill = CS.pcm_acc_status == AccState.STANDSTILL
|
||||
at_full_stop = enabled and standstill and car_stopping
|
||||
near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) and car_stopping
|
||||
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))
|
||||
|
||||
# Auto-resume from full stop by resetting ACC control
|
||||
acc_enabled = enabled
|
||||
if standstill and not car_stopping:
|
||||
acc_enabled = False
|
||||
|
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, acc_enabled, 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:
|
||||
@@ -163,10 +161,21 @@ class CarController(object):
|
||||
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)
|
||||
|
||||
# Show green icon when LKA torque is applied, and
|
||||
# alarming orange icon when approaching torque limit.
|
||||
# If not sent again, LKA icon disappears in about 5 seconds.
|
||||
# Conveniently, sending camera message periodically also works as a keepalive.
|
||||
lka_active = CS.lkas_status == 1
|
||||
lka_critical = lka_active and abs(actuators.steer) > 0.9
|
||||
lka_icon_status = (lka_active, lka_critical)
|
||||
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
|
||||
or lka_icon_status != self.lka_icon_status_last:
|
||||
can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical))
|
||||
self.lka_icon_status_last = lka_icon_status
|
||||
|
||||
# Send chimes
|
||||
if self.chime != chime:
|
||||
duration = 0x3c
|
||||
|
||||
@@ -60,15 +60,17 @@ def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_st
|
||||
|
||||
def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_full_stop):
|
||||
|
||||
mode = 0x1
|
||||
if apply_brake > 0:
|
||||
if apply_brake == 0:
|
||||
mode = 0x1
|
||||
else:
|
||||
mode = 0xa
|
||||
|
||||
if near_stop:
|
||||
mode = 0xb
|
||||
|
||||
if at_full_stop:
|
||||
mode = 0xd
|
||||
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
|
||||
@@ -132,6 +134,16 @@ 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]
|
||||
|
||||
def create_lka_icon_command(bus, active, critical):
|
||||
if active:
|
||||
if critical:
|
||||
dat = "\x40\xc0\x14"
|
||||
else:
|
||||
dat = "\x40\x40\x18"
|
||||
else:
|
||||
dat = "\x00\x00\x00"
|
||||
return [0x104c006c, 0, dat, bus]
|
||||
|
||||
# TODO: WIP
|
||||
'''
|
||||
def create_friction_brake_command_ct6(packer, bus, apply_brake, idx, near_stop, at_full_stop):
|
||||
|
||||
@@ -193,7 +193,7 @@ class CarInterface(object):
|
||||
ret.cruiseState.available = bool(self.CS.main_on)
|
||||
cruiseEnabled = self.CS.pcm_acc_status != 0
|
||||
ret.cruiseState.enabled = cruiseEnabled
|
||||
ret.cruiseState.standstill = False
|
||||
ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
|
||||
|
||||
ret.leftBlinker = self.CS.left_blinker_on
|
||||
ret.rightBlinker = self.CS.right_blinker_on
|
||||
@@ -276,6 +276,8 @@ class CarInterface(object):
|
||||
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:
|
||||
|
||||
@@ -12,12 +12,6 @@ class CruiseButtons:
|
||||
MAIN = 5
|
||||
CANCEL = 6
|
||||
|
||||
class AccState:
|
||||
OFF = 0
|
||||
ACTIVE = 1
|
||||
FAULTED = 3
|
||||
STANDSTILL = 4
|
||||
|
||||
def is_eps_status_ok(eps_status, car_fingerprint):
|
||||
valid_eps_status = []
|
||||
if car_fingerprint == CAR.VOLT:
|
||||
@@ -55,7 +49,6 @@ FINGERPRINTS = {
|
||||
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
|
||||
STOCK_CONTROL_MSGS = {
|
||||
CAR.VOLT: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
CAR.CADILLAC_CT6: [], # Cadillac does not require ASCMs to be disconnected
|
||||
|
||||
@@ -10,7 +10,6 @@ from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET,
|
||||
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, CAR, HONDA_BOSCH
|
||||
from selfdrive.controls.lib.planner import A_ACC_MAX
|
||||
|
||||
try:
|
||||
from selfdrive.car.honda.carcontroller import CarController
|
||||
@@ -128,7 +127,7 @@ class CarInterface(object):
|
||||
# accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
|
||||
# unless aTargetMax is very high and then we scale with it; this help in quicker restart
|
||||
|
||||
return float(max(0.714, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
|
||||
return float(min(speedLimiter, accelLimiter))
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint):
|
||||
|
||||
@@ -90,7 +90,7 @@ FINGERPRINTS = {
|
||||
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 334: 8, 339: 7, 342: 6, 344: 8, 379: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 837: 5, 856: 7, 871: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1618: 5, 1668: 5
|
||||
}],
|
||||
CAR.PILOT_2019: [{
|
||||
57: 3, 145: 8, 228: 5, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 983: 8, 985: 3, 1029: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
|
||||
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 392: 6, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1039: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
|
||||
}],
|
||||
# Ridgeline w/ Added Comma Pedal Support (512L & 513L)
|
||||
CAR.RIDGELINE: [{
|
||||
|
||||
@@ -44,6 +44,11 @@ def get_can_parser(CP):
|
||||
("CF_Clu_AmpInfo", "CLU11", 0),
|
||||
("CF_Clu_AliveCnt1", "CLU11", 0),
|
||||
|
||||
("CF_Clu_InhibitD", "CLU15", 0),
|
||||
("CF_Clu_InhibitP", "CLU15", 0),
|
||||
("CF_Clu_InhibitN", "CLU15", 0),
|
||||
("CF_Clu_InhibitR", "CLU15", 0),
|
||||
|
||||
("CF_Lvr_Gear","LVR12",0),
|
||||
|
||||
("ACCEnable", "TCS13", 0),
|
||||
@@ -204,7 +209,7 @@ class CarState(object):
|
||||
self.pedal_gas = cp.vl["EMS12"]['TPS']
|
||||
self.car_gas = cp.vl["EMS12"]['TPS']
|
||||
|
||||
# Gear Selecton - This should be compatible with all Kia/Hyundai with Auto's
|
||||
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
|
||||
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
|
||||
if gear == 5:
|
||||
self.gear_shifter = "drive"
|
||||
@@ -217,6 +222,18 @@ class CarState(object):
|
||||
else:
|
||||
self.gear_shifter = "unknown"
|
||||
|
||||
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method.
|
||||
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
|
||||
self.gear_shifter_cluster = "drive"
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
|
||||
self.gear_shifter_cluster = "neutral"
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
|
||||
self.gear_shifter_cluster = "park"
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
|
||||
self.gear_shifter_cluster = "reverse"
|
||||
else:
|
||||
self.gear_shifter_cluster = "unknown"
|
||||
|
||||
# save the entire LKAS11 and CLU11
|
||||
self.lkas11 = cp_cam.vl["LKAS11"]
|
||||
self.clu11 = cp.vl["CLU11"]
|
||||
|
||||
@@ -95,14 +95,15 @@ class CarInterface(object):
|
||||
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
|
||||
ret.minSteerSpeed = 0.
|
||||
elif candidate == CAR.ELANTRA:
|
||||
ret.steerKf = 0.00004
|
||||
ret.steerKf = 0.00006
|
||||
ret.steerRateCost = 0.5
|
||||
ret.mass = 1275 + std_cargo
|
||||
ret.wheelbase = 2.7
|
||||
ret.steerRatio = 16.9
|
||||
ret.steerRatio = 13.73 #Spec
|
||||
tire_stiffness_factor = 0.385
|
||||
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
|
||||
ret.steerKpV, ret.steerKiV = [[0.20], [0.01]]
|
||||
ret.minSteerSpeed = 35 * CV.MPH_TO_MS
|
||||
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
|
||||
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
|
||||
elif candidate == CAR.GENESIS:
|
||||
ret.steerKf = 0.00005
|
||||
ret.steerRateCost = 0.5
|
||||
@@ -190,7 +191,10 @@ class CarInterface(object):
|
||||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gear shifter
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
if self.CP.carFingerprint == CAR.ELANTRA:
|
||||
ret.gearShifter = self.CS.gear_shifter_cluster
|
||||
else:
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
# gas pedal
|
||||
ret.gas = self.CS.car_gas
|
||||
|
||||
@@ -10,24 +10,30 @@ import selfdrive.messaging as messaging
|
||||
from selfdrive.car.toyota.values import NO_DSU_CAR
|
||||
|
||||
|
||||
RADAR_MSGS = list(range(0x210, 0x220))
|
||||
RADAR_A_MSGS = list(range(0x210, 0x220))
|
||||
RADAR_B_MSGS = list(range(0x220, 0x230))
|
||||
|
||||
def _create_radard_can_parser():
|
||||
dbc_f = 'toyota_prius_2017_adas.dbc'
|
||||
msg_n = len(RADAR_MSGS)
|
||||
signals = zip(['LONG_DIST'] * msg_n + ['NEW_TRACK'] * msg_n + ['LAT_DIST'] * msg_n +
|
||||
['REL_SPEED'] * msg_n + ['VALID'] * msg_n,
|
||||
RADAR_MSGS * 5,
|
||||
[255] * msg_n + [1] * msg_n + [0] * msg_n + [0] * msg_n + [0] * msg_n)
|
||||
checks = zip(RADAR_MSGS, [20]*msg_n)
|
||||
|
||||
msg_a_n = len(RADAR_A_MSGS)
|
||||
msg_b_n = len(RADAR_B_MSGS)
|
||||
|
||||
signals = zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n +
|
||||
['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n,
|
||||
RADAR_A_MSGS * 5 + RADAR_B_MSGS,
|
||||
[255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n)
|
||||
|
||||
checks = zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_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.seen_valid = {key: False for key in RADAR_A_MSGS}
|
||||
self.track_id = 0
|
||||
|
||||
self.delay = 0.0 # Delay of radar
|
||||
@@ -52,7 +58,7 @@ class RadarInterface(object):
|
||||
tm = int(sec_since_boot() * 1e9)
|
||||
updated_messages.update(self.rcp.update(tm, True))
|
||||
# TODO: do not hardcode last msg
|
||||
if 0x21f in updated_messages:
|
||||
if 0x22f in updated_messages:
|
||||
break
|
||||
|
||||
errors = []
|
||||
@@ -62,32 +68,33 @@ class RadarInterface(object):
|
||||
ret.canMonoTimes = canMonoTimes
|
||||
|
||||
for ii in updated_messages:
|
||||
cpt = self.rcp.vl[ii]
|
||||
if ii in RADAR_A_MSGS:
|
||||
cpt = self.rcp.vl[ii]
|
||||
|
||||
if cpt['LONG_DIST'] >=255 or cpt['NEW_TRACK']:
|
||||
self.validCnt[ii] = 0 # reset counter
|
||||
if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
|
||||
self.seen_valid[ii] = False # reset validity
|
||||
|
||||
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
|
||||
self.validCnt[ii] += 1
|
||||
else:
|
||||
self.validCnt[ii] = max(self.validCnt[ii] -1, 0)
|
||||
#print ii, self.validCnt[ii], cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
|
||||
if cpt['LONG_DIST'] < 255 and cpt['VALID']:
|
||||
self.seen_valid[ii] = True
|
||||
|
||||
# radar point only valid if there have been enough valid measurements
|
||||
if self.validCnt[ii] > 0:
|
||||
if ii not in self.pts or cpt['NEW_TRACK']:
|
||||
self.pts[ii] = car.RadarState.RadarPoint.new_message()
|
||||
self.pts[ii].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
|
||||
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
|
||||
self.pts[ii].vRel = cpt['REL_SPEED']
|
||||
self.pts[ii].aRel = float('nan')
|
||||
self.pts[ii].yvRel = float('nan')
|
||||
self.pts[ii].measured = bool(cpt['VALID'])
|
||||
else:
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
score = self.rcp.vl[ii+16]['SCORE']
|
||||
# print ii, score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
|
||||
|
||||
# radar point only valid if it's a valid measurement and score is above 50
|
||||
if (cpt['VALID'] or score > 50) and cpt['LONG_DIST'] < 255 and self.seen_valid[ii]:
|
||||
if ii not in self.pts or cpt['NEW_TRACK']:
|
||||
self.pts[ii] = car.RadarState.RadarPoint.new_message()
|
||||
self.pts[ii].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
|
||||
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
|
||||
self.pts[ii].vRel = cpt['REL_SPEED']
|
||||
self.pts[ii].aRel = float('nan')
|
||||
self.pts[ii].yvRel = float('nan')
|
||||
self.pts[ii].measured = bool(cpt['VALID'])
|
||||
else:
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
|
||||
ret.points = self.pts.values()
|
||||
return ret
|
||||
|
||||
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "0.5.4-release"
|
||||
#define COMMA_VERSION "0.5.5-release"
|
||||
|
||||
@@ -352,6 +352,7 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac
|
||||
"alertSize": AM.alert_size,
|
||||
"alertStatus": AM.alert_status,
|
||||
"alertBlinkingRate": AM.alert_rate,
|
||||
"alertType": AM.alert_type,
|
||||
"awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
|
||||
"driverMonitoringOn": bool(driver_status.monitor_on),
|
||||
"canMonoTimes": list(CS.canMonoTimes),
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -4,6 +4,7 @@ CXX = clang++
|
||||
|
||||
PHONELIBS = ../../../../phonelibs
|
||||
|
||||
UNAME_S := $(shell uname -s)
|
||||
UNAME_M := $(shell uname -m)
|
||||
|
||||
CFLAGS = -O3 -fPIC -I.
|
||||
@@ -13,10 +14,12 @@ QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONEL
|
||||
|
||||
ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
|
||||
|
||||
ifeq ($(UNAME_M),aarch64)
|
||||
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||
ifeq ($(UNAME_S),Darwin)
|
||||
ACADO_LIBS := -lacado_toolkit_s
|
||||
else ifeq ($(UNAME_M),aarch64)
|
||||
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||
else
|
||||
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||
endif
|
||||
|
||||
OBJS = \
|
||||
@@ -67,7 +70,7 @@ lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
|
||||
-c -o '$@' '$<'
|
||||
|
||||
generator: generator.cpp
|
||||
$(CXX) -Wall -std=c++11 \
|
||||
$(CXX) -v -Wall -std=c++11 \
|
||||
generator.cpp \
|
||||
-o generator \
|
||||
$(ACADO_FLAGS) \
|
||||
|
||||
@@ -3,6 +3,7 @@ CXX = clang++
|
||||
|
||||
PHONELIBS = ../../../../phonelibs
|
||||
|
||||
UNAME_S := $(shell uname -s)
|
||||
UNAME_M := $(shell uname -m)
|
||||
|
||||
CFLAGS = -O3 -fPIC -I.
|
||||
@@ -12,10 +13,12 @@ QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONEL
|
||||
|
||||
ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
|
||||
|
||||
ifeq ($(UNAME_M),aarch64)
|
||||
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||
ifeq ($(UNAME_S),Darwin)
|
||||
ACADO_LIBS := -lacado_toolkit_s
|
||||
else ifeq ($(UNAME_M),aarch64)
|
||||
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||
else
|
||||
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
|
||||
endif
|
||||
|
||||
OBJS = \
|
||||
|
||||
@@ -44,9 +44,6 @@ _A_TOTAL_MAX_BP = [0., 20., 40.]
|
||||
_FCW_A_ACT_V = [-3., -2.]
|
||||
_FCW_A_ACT_BP = [0., 30.]
|
||||
|
||||
# max acceleration allowed in acc, which happens in restart
|
||||
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
|
||||
|
||||
|
||||
def calc_cruise_accel_limits(v_ego, following):
|
||||
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
||||
|
||||
@@ -142,14 +142,16 @@ def gen_solution(msg):
|
||||
'longitude': msg_data['lon']*1e-07, # longitude in degrees
|
||||
'speed': msg_data['gSpeed']*1e-03, # ground speed in meters
|
||||
'accuracy': msg_data['hAcc']*1e-03, # horizontal accuracy (1 sigma?)
|
||||
'timestamp': timestamp, # UTC time in ms since start of UTC stime
|
||||
'timestamp': timestamp, # UTC time in ms since start of UTC stime
|
||||
'vNED': [msg_data['velN']*1e-03,
|
||||
msg_data['velE']*1e-03,
|
||||
msg_data['velD']*1e-03], # velocity in NED frame in m/s
|
||||
'speedAccuracy': msg_data['sAcc']*1e-03, # speed accuracy in m/s
|
||||
'verticalAccuracy': msg_data['vAcc']*1e-03, # vertical accuracy in meters
|
||||
'bearingAccuracy': msg_data['headAcc']*1e-05, # heading accuracy in degrees
|
||||
'source': 'ublox'}
|
||||
'source': 'ublox',
|
||||
'flags': msg_data['flags'],
|
||||
}
|
||||
return log.Event.new_message(gpsLocationExternal=gps_fix)
|
||||
|
||||
def gen_nav_data(msg, nav_frame_buffer):
|
||||
|
||||
Binary file not shown.
@@ -7,7 +7,10 @@ from common.api import api_get
|
||||
from common.params import Params
|
||||
|
||||
def get_imei():
|
||||
return subprocess.check_output(["getprop", "oem.device.imeicache"]).strip()
|
||||
ret = subprocess.check_output(["getprop", "oem.device.imeicache"]).strip()
|
||||
if ret == "":
|
||||
ret = "000000000000000"
|
||||
return ret
|
||||
|
||||
def get_serial():
|
||||
return subprocess.check_output(["getprop", "ro.serialno"]).strip()
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -70,6 +70,7 @@ uiLayoutState: [8060, true]
|
||||
frontEncodeIdx: [8061, true]
|
||||
orbFeaturesSummary: [8062, true]
|
||||
driverMonitoring: [8063, true]
|
||||
liveParameters: [8064, true]
|
||||
|
||||
testModel: [8040, false]
|
||||
testLiveLocation: [8045, false]
|
||||
|
||||
Binary file not shown.
Reference in New Issue
Block a user