mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-24 23:42:05 +08:00
openpilot v0.4.1 release
This commit is contained in:
Vehicle Researcher
@@ -29,6 +29,9 @@ Supported Cars
|
||||
- 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
|
||||
|
||||
- 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
|
||||
@@ -42,6 +45,10 @@ Supported Cars
|
||||
- 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!)
|
||||
- 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
|
||||
|
||||
In Progress Cars
|
||||
------
|
||||
- Probably all TSS-P Toyota with Steering Assist.
|
||||
@@ -55,8 +62,6 @@ Community WIP Cars
|
||||
|
||||
- [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145)
|
||||
|
||||
- [Honda Odyssey 2018 with Honda Sensing](https://github.com/commaai/openpilot/pull/155)
|
||||
|
||||
- [Honda Pilot 2017 with Honda Sensing](https://github.com/commaai/openpilot/pull/161)
|
||||
|
||||
- [Acura RDX 2018 with AcuraWatch Plus](https://github.com/commaai/openpilot/pull/162)
|
||||
@@ -79,7 +84,6 @@ Directory structure
|
||||
- logcatd -- Android logcat as a service
|
||||
- loggerd -- Logger and uploader of car data
|
||||
- proclogd -- Logs information from proc
|
||||
- radar -- Code that talks to the radar and implements RadarInterface
|
||||
- sensord -- IMU / GPS interface code
|
||||
- test/plant -- Car simulator running code through virtual maneuvers
|
||||
- ui -- The UI
|
||||
|
||||
+7
-1
@@ -97,6 +97,11 @@ struct CarState {
|
||||
buttonEvents @11 :List(ButtonEvent);
|
||||
leftBlinker @20 :Bool;
|
||||
rightBlinker @21 :Bool;
|
||||
genericToggle @23 :Bool;
|
||||
|
||||
# lock info
|
||||
doorOpen @24 :Bool;
|
||||
seatbeltUnlatched @25 :Bool;
|
||||
|
||||
# which packets this state came from
|
||||
canMonoTimes @12: List(UInt64);
|
||||
@@ -250,7 +255,7 @@ struct CarControl {
|
||||
|
||||
struct CarParams {
|
||||
carName @0 :Text;
|
||||
radarName @1 :Text;
|
||||
radarNameDEPRECATED @1 :Text;
|
||||
carFingerprint @2 :Text;
|
||||
|
||||
enableSteer @3 :Bool;
|
||||
@@ -263,6 +268,7 @@ struct CarParams {
|
||||
|
||||
minEnableSpeed @17 :Float32;
|
||||
safetyModel @18 :Int16;
|
||||
safetyParam @41 :Int16;
|
||||
|
||||
steerMaxBP @19 :List(Float32);
|
||||
steerMaxV @20 :List(Float32);
|
||||
|
||||
@@ -573,6 +573,16 @@ struct LiveLocationData {
|
||||
|
||||
accuracy @13 :Accuracy;
|
||||
|
||||
source @14 :SensorSource;
|
||||
# if we are fixing a location in the past
|
||||
fixMonoTime @15 :UInt64;
|
||||
|
||||
gpsWeek @16 :Int32;
|
||||
timeOfWeek @17 :Float64;
|
||||
|
||||
positionECEF @18 :List(Float64);
|
||||
poseQuatECEF @19 :List(Float32);
|
||||
|
||||
struct Accuracy {
|
||||
pNEDError @0 :List(Float32);
|
||||
vNEDError @1 :List(Float32);
|
||||
@@ -583,6 +593,13 @@ struct LiveLocationData {
|
||||
ellipsoidSemiMinorError @6 :Float32;
|
||||
ellipsoidOrientationError @7 :Float32;
|
||||
}
|
||||
|
||||
enum SensorSource {
|
||||
applanix @0;
|
||||
kalman @1;
|
||||
orbslam @2;
|
||||
timing @3;
|
||||
}
|
||||
}
|
||||
|
||||
struct EthernetPacket {
|
||||
@@ -1331,6 +1348,34 @@ struct GPSPlannerPlan {
|
||||
valid @0 :Bool;
|
||||
poly @1 :List(Float32);
|
||||
trackName @2 :Text;
|
||||
speed @3 :Float32;
|
||||
}
|
||||
|
||||
struct TrafficSigns {
|
||||
type @0 :Type;
|
||||
distance @1 :Float32;
|
||||
action @2 :Action;
|
||||
resuming @3 :Bool;
|
||||
|
||||
enum Type {
|
||||
light @0;
|
||||
}
|
||||
|
||||
enum Action {
|
||||
none @0;
|
||||
yield @1;
|
||||
stop @2;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
struct OrbslamCorrection {
|
||||
correctionMonoTime @0 :UInt64;
|
||||
prePositionECEF @1 :List(Float64);
|
||||
postPositionECEF @2 :List(Float64);
|
||||
prePoseQuatECEF @3 :List(Float32);
|
||||
postPoseQuatECEF @4 :List(Float32);
|
||||
numInliers @5 :UInt32;
|
||||
}
|
||||
|
||||
struct Event {
|
||||
@@ -1379,5 +1424,10 @@ struct Event {
|
||||
ubloxRaw @39 :Data;
|
||||
gpsPlannerPoints @40 :GPSPlannerPoints;
|
||||
gpsPlannerPlan @41 :GPSPlannerPlan;
|
||||
applanixRaw @42 :Data;
|
||||
trafficSigns @43 :List(TrafficSigns);
|
||||
liveLocationTiming @44 :LiveLocationData;
|
||||
orbslamCorrection @45 :OrbslamCorrection;
|
||||
liveLocationCorrected @46 :LiveLocationData;
|
||||
}
|
||||
}
|
||||
|
||||
+19
-3
@@ -2,6 +2,8 @@ import re
|
||||
import os
|
||||
import struct
|
||||
import bitstring
|
||||
import sys
|
||||
import numbers
|
||||
from collections import namedtuple
|
||||
|
||||
def int_or_float(s):
|
||||
@@ -49,6 +51,8 @@ class dbc(object):
|
||||
name = dat.group(2)
|
||||
size = int(dat.group(3))
|
||||
ids = int(dat.group(1), 0) # could be hex
|
||||
if ids in self.msgs:
|
||||
sys.exit("Duplicate address detected %d %s" % (ids, self.name))
|
||||
|
||||
self.msgs[ids] = ((name, size), [])
|
||||
|
||||
@@ -80,6 +84,16 @@ class dbc(object):
|
||||
for msg in self.msgs.viewvalues():
|
||||
msg[1].sort(key=lambda x: x.start_bit)
|
||||
|
||||
self.msg_name_to_address = {}
|
||||
for address, m in self.msgs.items():
|
||||
name = m[0][0]
|
||||
self.msg_name_to_address[name] = address
|
||||
|
||||
def lookup_msg_id(self, msg_id):
|
||||
if not isinstance(msg_id, numbers.Number):
|
||||
msg_id = self.msg_name_to_address[msg_id]
|
||||
return msg_id
|
||||
|
||||
def encode(self, msg_id, dd):
|
||||
"""Encode a CAN message using the dbc.
|
||||
|
||||
@@ -87,6 +101,8 @@ class dbc(object):
|
||||
msg_id: The message ID.
|
||||
dd: A dictionary mapping signal name to signal data.
|
||||
"""
|
||||
msg_id = self.lookup_msg_id(msg_id)
|
||||
|
||||
# TODO: Stop using bitstring, which is super slow.
|
||||
msg_def = self.msgs[msg_id]
|
||||
size = msg_def[0][1]
|
||||
@@ -134,7 +150,7 @@ class dbc(object):
|
||||
|
||||
Returns (None, None) if the message could not be decoded.
|
||||
"""
|
||||
|
||||
|
||||
if arr is None:
|
||||
out = {}
|
||||
else:
|
||||
@@ -193,10 +209,10 @@ class dbc(object):
|
||||
out[arr.index(s[0])] = ival
|
||||
return name, out
|
||||
|
||||
|
||||
def get_signals(self, msg):
|
||||
msg = self.lookup_msg_id(msg)
|
||||
return [sgs.name for sgs in self.msgs[msg][1]]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import sys
|
||||
import os
|
||||
|
||||
+48
-16
@@ -1,33 +1,57 @@
|
||||
import os
|
||||
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"
|
||||
|
||||
|
||||
class TOYOTA:
|
||||
PRIUS = "TOYOTA PRIUS 2017"
|
||||
RAV4H = "TOYOTA RAV4 2017 HYBRID"
|
||||
RAV4 = "TOYOTA RAV4 2017"
|
||||
COROLLA = "TOYOTA COROLLA 2017"
|
||||
|
||||
|
||||
_FINGERPRINTS = {
|
||||
"ACURA ILX 2016 ACURAWATCH PLUS": {
|
||||
HONDA.ACURA_ILX: {
|
||||
1024L: 5, 513L: 5, 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: 3, 0x30c: 8, 0x33d: 5,
|
||||
},
|
||||
"HONDA CIVIC 2016 TOURING": {
|
||||
HONDA.CIVIC: {
|
||||
1024L: 5, 513L: 5, 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: 3, 0x30c: 8, 0x33d: 5, 0x35e: 8, 0x39f: 8,
|
||||
},
|
||||
"HONDA CR-V 2016 TOURING": {
|
||||
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 2018 EX-L": {
|
||||
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
|
||||
},
|
||||
"TOYOTA RAV4 2017": {
|
||||
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, 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, 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 RAV4 2017 HYBRID": {
|
||||
TOYOTA.RAV4H: {
|
||||
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: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 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, 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, 581L: 5, 296: 8, 552L: 8, 560L: 7, 552L: 4, 713L: 8, 550L: 8, 608L: 8, 37L: 8, 36L: 8, 950L: 8, 1198L: 8, 1197L: 8, 1199L: 8, 1212L: 8, 953L: 3, 1264L: 8, 1184L: 8, 1005L: 2, 1185L: 8, 1232L: 8, 1186L: 8
|
||||
},
|
||||
"TOYOTA PRIUS 2017": {
|
||||
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, 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
|
||||
}
|
||||
],
|
||||
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
|
||||
},
|
||||
}
|
||||
|
||||
# support additional internal only fingerprints
|
||||
@@ -37,6 +61,12 @@ try:
|
||||
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))
|
||||
|
||||
|
||||
def eliminate_incompatible_cars(msg, candidate_cars):
|
||||
"""Removes cars that could not have sent msg.
|
||||
|
||||
@@ -49,16 +79,18 @@ def eliminate_incompatible_cars(msg, candidate_cars):
|
||||
"""
|
||||
compatible_cars = []
|
||||
for car_name in candidate_cars:
|
||||
adr = msg.address
|
||||
if msg.src != 0 or (adr in _FINGERPRINTS[car_name] and
|
||||
_FINGERPRINTS[car_name][adr] == len(msg.dat)):
|
||||
compatible_cars.append(car_name)
|
||||
else:
|
||||
pass
|
||||
#isin = adr in _FINGERPRINTS[car_name]
|
||||
#print "eliminate", car_name, hex(adr), isin, len(msg.dat), msg.dat.encode("hex")
|
||||
car_fingerprints = _FINGERPRINTS[car_name]
|
||||
if not isinstance(car_fingerprints, list):
|
||||
car_fingerprints = [car_fingerprints]
|
||||
|
||||
for fingerprint in car_fingerprints:
|
||||
if is_valid_for_fingerprint(msg, fingerprint):
|
||||
compatible_cars.append(car_name)
|
||||
break
|
||||
|
||||
return compatible_cars
|
||||
|
||||
|
||||
def all_known_cars():
|
||||
"""Returns a list of all known car strings."""
|
||||
return _FINGERPRINTS.keys()
|
||||
|
||||
@@ -1,3 +1,7 @@
|
||||
def int_rnd(x):
|
||||
return int(round(x))
|
||||
|
||||
|
||||
def clip(x, lo, hi):
|
||||
return max(lo, min(hi, x))
|
||||
|
||||
|
||||
@@ -11,5 +11,7 @@ simplejson==3.8.2
|
||||
pyyaml==3.12
|
||||
cffi==1.7.0
|
||||
enum34==1.1.1
|
||||
sympy==1.1.1
|
||||
filterpy==1.0.0
|
||||
smbus2==0.2.0
|
||||
-e git+https://github.com/commaai/le_python.git#egg=Logentries
|
||||
|
||||
+97
-39
@@ -50,6 +50,11 @@ bool loopback_can = false;
|
||||
bool has_pigeon = false;
|
||||
|
||||
pthread_t safety_setter_thread_handle = -1;
|
||||
pthread_t pigeon_thread_handle = -1;
|
||||
bool pigeon_needs_init;
|
||||
|
||||
void pigeon_init();
|
||||
void *pigeon_thread(void *crap);
|
||||
|
||||
void *safety_setter_thread(void *s) {
|
||||
char *value;
|
||||
@@ -73,7 +78,8 @@ void *safety_setter_thread(void *s) {
|
||||
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
|
||||
|
||||
auto safety_model = car_params.getSafetyModel();
|
||||
LOGW("setting safety model: %d", safety_model);
|
||||
auto safety_param = car_params.getSafetyParam();
|
||||
LOGW("setting safety model: %d with param %d", safety_model, safety_param);
|
||||
|
||||
int safety_setting = 0;
|
||||
switch (safety_model) {
|
||||
@@ -98,18 +104,17 @@ void *safety_setter_thread(void *s) {
|
||||
// set in the mutex to avoid race
|
||||
safety_setter_thread_handle = -1;
|
||||
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_setting, 0, NULL, 0, TIMEOUT);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_setting, safety_param, NULL, 0, TIMEOUT);
|
||||
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void pigeon_init();
|
||||
|
||||
// must be called before threads or with mutex
|
||||
bool usb_connect() {
|
||||
int err;
|
||||
unsigned char is_pigeon[1] = {0};
|
||||
|
||||
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
|
||||
if (dev_handle == NULL) { goto fail; }
|
||||
@@ -143,9 +148,19 @@ bool usb_connect() {
|
||||
|
||||
if (safety_setter_thread_handle == -1) {
|
||||
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
|
||||
assert(err == 0);
|
||||
}
|
||||
|
||||
if (has_pigeon) pigeon_init();
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, is_pigeon, 1, TIMEOUT);
|
||||
|
||||
if (is_pigeon[0]) {
|
||||
LOGW("grey panda detected");
|
||||
pigeon_needs_init = true;
|
||||
if (pigeon_thread_handle == -1) {
|
||||
err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL);
|
||||
assert(err == 0);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
fail:
|
||||
@@ -418,42 +433,72 @@ void *can_health_thread(void *crap) {
|
||||
|
||||
#define pigeon_send(x) _pigeon_send(x, sizeof(x)-1)
|
||||
|
||||
void hexdump(unsigned char *d, int l) {
|
||||
for (int i = 0; i < l; i++) {
|
||||
if (i!=0 && i%0x10 == 0) printf("\n");
|
||||
printf("%2.2X ", d[i]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
void _pigeon_send(const char *dat, int len) {
|
||||
int sent;
|
||||
unsigned char a[0x20];
|
||||
int err;
|
||||
a[0] = 1;
|
||||
for (int i=0; i<len; i+=0x1f) {
|
||||
int ll = std::max(0x1f, len-i);
|
||||
for (int i=0; i<len; i+=0x20) {
|
||||
int ll = std::min(0x20, len-i);
|
||||
memcpy(&a[1], &dat[i], ll);
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
err = libusb_bulk_transfer(dev_handle, 2, a, ll+1, &sent, TIMEOUT);
|
||||
assert(err == 0);
|
||||
assert(sent == ll+1);
|
||||
//hexdump(a, ll+1);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
}
|
||||
}
|
||||
|
||||
void pigeon_set_power(int power) {
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xd9, power, 0, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
}
|
||||
|
||||
void pigeon_set_baud(int baud) {
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xe2, 1, 0, NULL, 0, TIMEOUT);
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xe4, 1, baud/300, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
}
|
||||
|
||||
void pigeon_init() {
|
||||
// power on pigeon
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT);
|
||||
usleep(1000*1000);
|
||||
LOGW("pigeon start");
|
||||
|
||||
// power off pigeon
|
||||
pigeon_set_power(0);
|
||||
usleep(100*1000);
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xd9, 1, 0, NULL, 0, TIMEOUT);
|
||||
|
||||
// 9600 baud at init
|
||||
pigeon_set_baud(9600);
|
||||
|
||||
// power on pigeon
|
||||
pigeon_set_power(1);
|
||||
usleep(500*1000);
|
||||
|
||||
// baud rate upping
|
||||
pigeon_set_baud(9600);
|
||||
pigeon_send("$PUBX,41,1,0007,0003,230400,0*1A\r\n");
|
||||
usleep(200*1000);
|
||||
pigeon_send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A");
|
||||
usleep(100*1000);
|
||||
|
||||
// set baud rate to 230400
|
||||
pigeon_set_baud(230400);
|
||||
// set baud rate to 460800
|
||||
pigeon_set_baud(460800);
|
||||
usleep(100*1000);
|
||||
|
||||
// init from ubloxd
|
||||
pigeon_send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F");
|
||||
pigeon_send("\xB5\x62\x06\x3E\x00\x00\x44\xD2");
|
||||
pigeon_send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35");
|
||||
pigeon_send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x00\x00\x00\x00\x00\x00\x00\x00\xF2\x72");
|
||||
pigeon_send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80");
|
||||
pigeon_send("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85");
|
||||
pigeon_send("\xB5\x62\x06\x00\x00\x00\x06\x18");
|
||||
pigeon_send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22");
|
||||
@@ -469,42 +514,66 @@ void pigeon_init() {
|
||||
pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70");
|
||||
|
||||
LOGW("pigeon is ready to fly");
|
||||
|
||||
}
|
||||
|
||||
|
||||
void *pigeon_thread(void *crap) {
|
||||
// ubloxRaw = 8042
|
||||
void *context = zmq_ctx_new();
|
||||
void *publisher = zmq_socket(context, ZMQ_PUB);
|
||||
zmq_bind(publisher, "tcp://*:8042");
|
||||
|
||||
// run at ~200hz
|
||||
unsigned char dat[0x40];
|
||||
// run at ~100hz
|
||||
unsigned char dat[0x1000];
|
||||
uint64_t cnt = 0;
|
||||
while (!do_exit) {
|
||||
while (1) {
|
||||
if (pigeon_needs_init) {
|
||||
pigeon_needs_init = false;
|
||||
pigeon_init();
|
||||
} else {
|
||||
// send periodic messages
|
||||
if (cnt%3000 == 0) {
|
||||
for (unsigned char sv = 1; sv < 33; ++sv){
|
||||
const unsigned char buffer[5] = {0x0B, 0x31, 0x01, 0x00, sv};
|
||||
unsigned char CK_A = 0;
|
||||
unsigned char CK_B = 0;
|
||||
for(int i=0;i<5;i++) {
|
||||
CK_A = CK_A + buffer[i];
|
||||
CK_B = CK_B + CK_A;
|
||||
}
|
||||
const unsigned char msg[9] = {0xB5, 0x62, 0x0B, 0x31, 0x01, 0x00, sv, CK_A, CK_B};
|
||||
_pigeon_send((const char *)msg, 9);
|
||||
}
|
||||
pigeon_send("\xB5\x62\x0b\x02\x00\x00\x0d\x32");
|
||||
}
|
||||
}
|
||||
int alen = 0;
|
||||
while (alen < 0xfc0) {
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
int len = libusb_control_transfer(dev_handle, 0xc0, 0xe0, 1, 0, dat, 0x40, TIMEOUT);
|
||||
int len = libusb_control_transfer(dev_handle, 0xc0, 0xe0, 1, 0, dat+alen, 0x40, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
if (len <= 0) break;
|
||||
|
||||
//printf("got %d\n", len);
|
||||
alen += len;
|
||||
}
|
||||
if (alen > 0) {
|
||||
// create message
|
||||
capnp::MallocMessageBuilder msg;
|
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||
event.setLogMonoTime(nanos_since_boot());
|
||||
auto ublox_raw = event.initUbloxRaw(len);
|
||||
memcpy(ublox_raw.begin(), dat, len);
|
||||
auto ublox_raw = event.initUbloxRaw(alen);
|
||||
memcpy(ublox_raw.begin(), dat, alen);
|
||||
|
||||
// send to ubloxRaw
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
zmq_send(publisher, bytes.begin(), bytes.size(), 0);
|
||||
|
||||
if (len < 0x40) break;
|
||||
}
|
||||
|
||||
// 10ms
|
||||
usleep(10*1000);
|
||||
cnt++;
|
||||
}
|
||||
|
||||
return NULL;
|
||||
@@ -541,10 +610,6 @@ int main() {
|
||||
loopback_can = true;
|
||||
}
|
||||
|
||||
if (getenv("PIGEON")) {
|
||||
has_pigeon = true;
|
||||
}
|
||||
|
||||
// init libusb
|
||||
err = libusb_init(&ctx);
|
||||
assert(err == 0);
|
||||
@@ -575,15 +640,6 @@ int main() {
|
||||
thermal_thread, NULL);
|
||||
assert(err == 0);
|
||||
|
||||
if (has_pigeon) {
|
||||
pthread_t pigeon_thread_handle;
|
||||
err = pthread_create(&pigeon_thread_handle, NULL,
|
||||
pigeon_thread, NULL);
|
||||
assert(err == 0);
|
||||
err = pthread_join(pigeon_thread_handle, NULL);
|
||||
assert(err == 0);
|
||||
}
|
||||
|
||||
// join threads
|
||||
|
||||
err = pthread_join(thermal_thread_handle, NULL);
|
||||
@@ -598,6 +654,8 @@ int main() {
|
||||
err = pthread_join(can_health_thread_handle, NULL);
|
||||
assert(err == 0);
|
||||
|
||||
//while (!do_exit) usleep(1000);
|
||||
|
||||
// destruct libusb
|
||||
|
||||
libusb_close(dev_handle);
|
||||
|
||||
@@ -65,3 +65,5 @@ dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc process_dbc.py dbc_template.cc
|
||||
clean:
|
||||
rm -rf libdbc.so*
|
||||
rm -f dbc_out/*.cc
|
||||
rm -f dbcs.txt
|
||||
rm -f dbcs.csv
|
||||
|
||||
@@ -24,3 +24,9 @@ const DBC* dbc_lookup(const std::string& dbc_name) {
|
||||
void dbc_register(const DBC* dbc) {
|
||||
get_dbcs().push_back(dbc);
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
const DBC* dbc_lookup(const char* dbc_name) {
|
||||
return dbc_lookup(std::string(dbc_name));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -11,24 +11,60 @@ subprocess.check_call(["make"], stdout=sys.stderr, cwd=can_dir)
|
||||
ffi = FFI()
|
||||
ffi.cdef("""
|
||||
|
||||
typedef struct SignalParseOptions {
|
||||
typedef struct {
|
||||
const char* name;
|
||||
double value;
|
||||
} SignalPackValue;
|
||||
|
||||
typedef struct {
|
||||
uint32_t address;
|
||||
const char* name;
|
||||
double default_value;
|
||||
} SignalParseOptions;
|
||||
|
||||
typedef struct MessageParseOptions {
|
||||
typedef struct {
|
||||
uint32_t address;
|
||||
int check_frequency;
|
||||
} MessageParseOptions;
|
||||
|
||||
typedef struct SignalValue {
|
||||
typedef struct {
|
||||
uint32_t address;
|
||||
uint16_t ts;
|
||||
const char* name;
|
||||
double value;
|
||||
} SignalValue;
|
||||
|
||||
|
||||
typedef enum {
|
||||
DEFAULT,
|
||||
HONDA_CHECKSUM,
|
||||
HONDA_COUNTER,
|
||||
TOYOTA_CHECKSUM,
|
||||
} SignalType;
|
||||
|
||||
typedef struct {
|
||||
const char* name;
|
||||
int b1, b2, bo;
|
||||
bool is_signed;
|
||||
double factor, offset;
|
||||
SignalType type;
|
||||
} Signal;
|
||||
|
||||
typedef struct {
|
||||
const char* name;
|
||||
uint32_t address;
|
||||
unsigned int size;
|
||||
size_t num_sigs;
|
||||
const Signal *sigs;
|
||||
} Msg;
|
||||
|
||||
typedef struct {
|
||||
const char* name;
|
||||
size_t num_msgs;
|
||||
const Msg *msgs;
|
||||
} DBC;
|
||||
|
||||
|
||||
void* can_init(int bus, const char* dbc_name,
|
||||
size_t num_message_options, const MessageParseOptions* message_options,
|
||||
size_t num_signal_options, const SignalParseOptions* signal_options);
|
||||
@@ -37,11 +73,7 @@ void can_update(void* can, uint64_t sec, bool wait);
|
||||
|
||||
size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
|
||||
|
||||
|
||||
typedef struct SignalPackValue {
|
||||
const char* name;
|
||||
double value;
|
||||
} SignalPackValue;
|
||||
const DBC* dbc_lookup(const char* dbc_name);
|
||||
|
||||
void* canpack_init(const char* dbc_name);
|
||||
|
||||
|
||||
@@ -29,7 +29,7 @@ class CANPacker(object):
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
cp = CANPacker("honda_civic_touring_2016_can")
|
||||
cp = CANPacker("honda_civic_touring_2016_can_generated")
|
||||
s = cp.pack_bytes(0x30c, [
|
||||
("PCM_SPEED", 123),
|
||||
("PCM_GAS", 10),
|
||||
|
||||
+40
-4
@@ -1,6 +1,7 @@
|
||||
import os
|
||||
import time
|
||||
from collections import defaultdict
|
||||
import numbers
|
||||
|
||||
from selfdrive.can.libdbc_py import libdbc, ffi
|
||||
|
||||
@@ -10,8 +11,39 @@ class CANParser(object):
|
||||
self.vl = defaultdict(dict)
|
||||
self.ts = defaultdict(dict)
|
||||
|
||||
self.dbc = libdbc.dbc_lookup(dbc_name)
|
||||
self.msg_name_to_addres = {}
|
||||
self.address_to_msg_name = {}
|
||||
|
||||
num_msgs = self.dbc[0].num_msgs
|
||||
for i in range(num_msgs):
|
||||
msg = self.dbc[0].msgs[i]
|
||||
|
||||
name = ffi.string(msg.name)
|
||||
address = msg.address
|
||||
|
||||
self.msg_name_to_addres[name] = address
|
||||
self.address_to_msg_name[address] = name
|
||||
|
||||
# Convert message names into adresses
|
||||
for i in range(len(signals)):
|
||||
s = signals[i]
|
||||
if not isinstance(s[1], numbers.Number):
|
||||
s = (s[0], self.msg_name_to_addres[s[1]], s[2])
|
||||
signals[i] = s
|
||||
|
||||
for i in range(len(checks)):
|
||||
c = checks[i]
|
||||
if not isinstance(c[0], numbers.Number):
|
||||
c = (self.msg_name_to_addres[c[0]], c[1])
|
||||
checks[i] = c
|
||||
|
||||
sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals)
|
||||
|
||||
# Set default values by name
|
||||
for sig_name, sig_address, sig_default in signals:
|
||||
self.vl[self.address_to_msg_name[sig_address]][sig_name] = sig_default
|
||||
|
||||
signal_options_c = ffi.new("SignalParseOptions[]", [
|
||||
{
|
||||
'address': sig_address,
|
||||
@@ -21,7 +53,7 @@ class CANParser(object):
|
||||
|
||||
message_options = dict((address, 0) for _, address, _ in signals)
|
||||
message_options.update(dict(checks))
|
||||
|
||||
|
||||
message_options_c = ffi.new("MessageParseOptions[]", [
|
||||
{
|
||||
'address': address,
|
||||
@@ -54,6 +86,10 @@ class CANParser(object):
|
||||
name = ffi.string(cv.name)
|
||||
self.vl[address][name] = cv.value
|
||||
self.ts[address][name] = cv.ts
|
||||
|
||||
sig_name = self.address_to_msg_name[address]
|
||||
self.vl[sig_name][name] = cv.value
|
||||
self.ts[sig_name][name] = cv.ts
|
||||
ret.add(address)
|
||||
return ret
|
||||
|
||||
@@ -74,7 +110,7 @@ if __name__ == "__main__":
|
||||
|
||||
|
||||
# signals = [
|
||||
# ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default
|
||||
# ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default
|
||||
# ("WHEEL_SPEED_FL", 0x1d0, 0),
|
||||
# ("WHEEL_SPEED_FR", 0x1d0, 0),
|
||||
# ("WHEEL_SPEED_RL", 0x1d0, 0),
|
||||
@@ -123,7 +159,7 @@ if __name__ == "__main__":
|
||||
# (0x405, 3),
|
||||
# ]
|
||||
|
||||
# cp = CANParser("honda_civic_touring_2016_can", signals, checks, 0)
|
||||
# cp = CANParser("honda_civic_touring_2016_can_generated", signals, checks, 0)
|
||||
|
||||
|
||||
signals = [
|
||||
@@ -164,7 +200,7 @@ if __name__ == "__main__":
|
||||
(608, 50),
|
||||
]
|
||||
|
||||
cp = CANParser("toyota_rav4_2017_pt", signals, checks, 0)
|
||||
cp = CANParser("toyota_rav4_2017_pt_generated", signals, checks, 0)
|
||||
|
||||
# print cp.vl
|
||||
|
||||
|
||||
@@ -4,7 +4,7 @@ import sys
|
||||
|
||||
import jinja2
|
||||
|
||||
import opendbc
|
||||
from collections import Counter
|
||||
from common.dbc import dbc
|
||||
|
||||
if len(sys.argv) != 3:
|
||||
@@ -31,6 +31,13 @@ elif can_dbc.name.startswith("toyota"):
|
||||
else:
|
||||
checksum_type = None
|
||||
|
||||
|
||||
# Fail on duplicate messgae names
|
||||
c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs])
|
||||
for name, count in c.items():
|
||||
if count > 1:
|
||||
sys.exit("Duplicate message name in DBC file %s" % name)
|
||||
|
||||
parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, len=len)
|
||||
|
||||
with open(out_fn, "w") as out_f:
|
||||
|
||||
+11
-14
@@ -9,6 +9,7 @@ import selfdrive.messaging as messaging
|
||||
from selfdrive.car.honda.interface import CarInterface as HondaInterface
|
||||
from selfdrive.car.toyota.interface import CarInterface as ToyotaInterface
|
||||
from selfdrive.car.mock.interface import CarInterface as MockInterface
|
||||
from common.fingerprints import HONDA, TOYOTA
|
||||
|
||||
try:
|
||||
from .simulator.interface import CarInterface as SimInterface
|
||||
@@ -22,26 +23,23 @@ except ImportError:
|
||||
|
||||
|
||||
interfaces = {
|
||||
"HONDA CIVIC 2016 TOURING": HondaInterface,
|
||||
"ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface,
|
||||
"HONDA ACCORD 2016 TOURING": HondaInterface,
|
||||
"HONDA CR-V 2016 TOURING": HondaInterface,
|
||||
"HONDA ODYSSEY 2018 EX-L": HondaInterface,
|
||||
"TOYOTA PRIUS 2017": ToyotaInterface,
|
||||
"TOYOTA RAV4 2017": ToyotaInterface,
|
||||
"TOYOTA RAV4 2017 HYBRID": ToyotaInterface,
|
||||
HONDA.CIVIC: HondaInterface,
|
||||
HONDA.ACURA_ILX: HondaInterface,
|
||||
HONDA.CRV: HondaInterface,
|
||||
HONDA.ODYSSEY: HondaInterface,
|
||||
|
||||
TOYOTA.PRIUS: ToyotaInterface,
|
||||
TOYOTA.RAV4: ToyotaInterface,
|
||||
TOYOTA.RAV4H: ToyotaInterface,
|
||||
TOYOTA.COROLLA: ToyotaInterface,
|
||||
|
||||
"simulator": SimInterface,
|
||||
"simulator2": Sim2Interface,
|
||||
|
||||
"mock": MockInterface
|
||||
}
|
||||
|
||||
# **** for use live only ****
|
||||
def fingerprint(logcan, timeout):
|
||||
if os.getenv("SIMULATOR") is not None or logcan is None:
|
||||
return ("simulator", None)
|
||||
elif os.getenv("SIMULATOR2") is not None:
|
||||
if os.getenv("SIMULATOR2") is not None:
|
||||
return ("simulator2", None)
|
||||
|
||||
finger_st = sec_since_boot()
|
||||
@@ -77,7 +75,6 @@ def fingerprint(logcan, timeout):
|
||||
|
||||
|
||||
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)
|
||||
|
||||
@@ -1,18 +1,15 @@
|
||||
from collections import namedtuple
|
||||
import os
|
||||
import common.numpy_fast as np
|
||||
from common.numpy_fast import clip, interp
|
||||
from common.realtime import sec_since_boot
|
||||
|
||||
from selfdrive.config import CruiseButtons
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.controls.lib.drive_helpers import rate_limit
|
||||
|
||||
from common.realtime import sec_since_boot
|
||||
from common.numpy_fast import clip
|
||||
from . import hondacan
|
||||
from .values import AH
|
||||
from common.fingerprints import HONDA as CAR
|
||||
|
||||
|
||||
def actuator_hystereses(brake, braking, brake_steady, v_ego, civic, odyssey):
|
||||
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
|
||||
# hyst params... TODO: move these to VehicleParams
|
||||
brake_hyst_on = 0.02 # to activate brakes exceed this value
|
||||
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
||||
@@ -32,7 +29,7 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, civic, odyssey):
|
||||
brake_steady = brake + brake_hyst_gap
|
||||
brake = brake_steady
|
||||
|
||||
if (not civic and not odyssey) and brake > 0.0:
|
||||
if (car_fingerprint in (CAR.ACURA_ILX, CAR.CRV)) and brake > 0.0:
|
||||
brake += 0.15
|
||||
|
||||
return brake, braking, brake_steady
|
||||
@@ -71,14 +68,14 @@ class CarController(object):
|
||||
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
|
||||
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
|
||||
snd_beep, snd_chime):
|
||||
|
||||
""" Controls thread """
|
||||
|
||||
# TODO: Make the accord work.
|
||||
if CS.accord or not self.enable_camera:
|
||||
if not self.enable_camera:
|
||||
return
|
||||
|
||||
# *** apply brake hysteresis ***
|
||||
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.civic, CS.odyssey)
|
||||
brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint)
|
||||
|
||||
# *** no output if not enabled ***
|
||||
if not enabled and CS.pcm_acc_status:
|
||||
@@ -121,11 +118,11 @@ class CarController(object):
|
||||
tt = sec_since_boot()
|
||||
GAS_MAX = 1004
|
||||
BRAKE_MAX = 1024/4
|
||||
if CS.civic or CS.odyssey:
|
||||
if CS.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY):
|
||||
is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c']
|
||||
STEER_MAX = 0x1FFF if is_fw_modified else 0x1000
|
||||
elif CS.crv:
|
||||
STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value
|
||||
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
|
||||
else:
|
||||
STEER_MAX = 0xF00
|
||||
GAS_OFFSET = 328
|
||||
@@ -143,12 +140,8 @@ class CarController(object):
|
||||
can_sends = []
|
||||
|
||||
# Send steering command.
|
||||
if CS.accord:
|
||||
idx = frame % 2
|
||||
can_sends.append(hondacan.create_accord_steering_control(apply_steer, idx))
|
||||
else:
|
||||
idx = frame % 4
|
||||
can_sends.extend(hondacan.create_steering_control(apply_steer, CS.crv, idx))
|
||||
idx = frame % 4
|
||||
can_sends.extend(hondacan.create_steering_control(apply_steer, CS.CP.carFingerprint, idx))
|
||||
|
||||
# Send gas and brake commands.
|
||||
if (frame % 2) == 0:
|
||||
@@ -165,16 +158,16 @@ class CarController(object):
|
||||
# Send dashboard UI commands.
|
||||
if (frame % 10) == 0:
|
||||
idx = (frame/10) % 4
|
||||
can_sends.extend(hondacan.create_ui_commands(pcm_speed, hud, CS.civic, CS.accord, CS.crv, CS.odyssey, idx))
|
||||
can_sends.extend(hondacan.create_ui_commands(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.acura:
|
||||
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.civic, CS.accord, CS.crv, CS.odyssey, idx))
|
||||
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())
|
||||
|
||||
+155
-427
@@ -4,24 +4,26 @@ from common.numpy_fast import interp
|
||||
import selfdrive.messaging as messaging
|
||||
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
|
||||
import numpy as np
|
||||
|
||||
def parse_gear_shifter(can_gear_shifter, is_acura, is_odyssey):
|
||||
|
||||
def parse_gear_shifter(can_gear_shifter, car_fingerprint):
|
||||
# TODO: Use values from DBC to parse this field
|
||||
if can_gear_shifter == 0x1:
|
||||
return "park"
|
||||
elif can_gear_shifter == 0x2:
|
||||
return "reverse"
|
||||
|
||||
if is_acura or is_odyssey:
|
||||
if car_fingerprint in (CAR.ACURA_ILX, CAR.ODYSSEY):
|
||||
if can_gear_shifter == 0x3:
|
||||
return "neutral"
|
||||
elif can_gear_shifter == 0x4:
|
||||
return "drive"
|
||||
elif can_gear_shifter == 0xa:
|
||||
return "sport"
|
||||
|
||||
else:
|
||||
elif car_fingerprint in (CAR.CIVIC, CAR.CRV):
|
||||
if can_gear_shifter == 0x4:
|
||||
return "neutral"
|
||||
elif can_gear_shifter == 0x8:
|
||||
@@ -33,9 +35,6 @@ def parse_gear_shifter(can_gear_shifter, is_acura, is_odyssey):
|
||||
|
||||
return "unknown"
|
||||
|
||||
_K0 = -0.3
|
||||
_K1 = -0.01879
|
||||
_K2 = 0.01013
|
||||
|
||||
def calc_cruise_offset(offset, speed):
|
||||
# euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
|
||||
@@ -43,311 +42,102 @@ def calc_cruise_offset(offset, speed):
|
||||
# - speed = 0m/s, out = -0.3
|
||||
# - speed = 34m/s, offset = 20, out = -0.25
|
||||
# - speed = 34m/s, offset = -2.5, out = -1.8
|
||||
_K0 = -0.3
|
||||
_K1 = -0.01879
|
||||
_K2 = 0.01013
|
||||
return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.)
|
||||
|
||||
|
||||
def get_can_signals(CP):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
|
||||
dbc_f = 'honda_civic_touring_2016_can.dbc'
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("XMISSION_SPEED", 0x158, 0),
|
||||
("WHEEL_SPEED_FL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_FR", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RR", 0x1d0, 0),
|
||||
("STEER_ANGLE", 0x14a, 0),
|
||||
("STEER_ANGLE_RATE", 0x14a, 0),
|
||||
("STEER_TORQUE_SENSOR", 0x18f, 0),
|
||||
("GEAR", 0x191, 0),
|
||||
("WHEELS_MOVING", 0x1b0, 1),
|
||||
("DOOR_OPEN_FL", 0x405, 1),
|
||||
("DOOR_OPEN_FR", 0x405, 1),
|
||||
("DOOR_OPEN_RL", 0x405, 1),
|
||||
("DOOR_OPEN_RR", 0x405, 1),
|
||||
("CRUISE_SPEED_PCM", 0x324, 0),
|
||||
("SEATBELT_DRIVER_LAMP", 0x305, 1),
|
||||
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
|
||||
("BRAKE_PRESSED", 0x17c, 0),
|
||||
("BRAKE_SWITCH", 0x17c, 0),
|
||||
("CAR_GAS", 0x130, 0),
|
||||
("CRUISE_BUTTONS", 0x296, 0),
|
||||
("ESP_DISABLED", 0x1a4, 1),
|
||||
("HUD_LEAD", 0x30c, 0),
|
||||
("USER_BRAKE", 0x1a4, 0),
|
||||
("STEER_STATUS", 0x18f, 5),
|
||||
("BRAKE_ERROR_1", 0x1b0, 1),
|
||||
("BRAKE_ERROR_2", 0x1b0, 1),
|
||||
("GEAR_SHIFTER", 0x191, 0),
|
||||
("MAIN_ON", 0x326, 0),
|
||||
("ACC_STATUS", 0x17c, 0),
|
||||
("PEDAL_GAS", 0x17c, 0),
|
||||
("CRUISE_SETTING", 0x296, 0),
|
||||
("LEFT_BLINKER", 0x326, 0),
|
||||
("RIGHT_BLINKER", 0x326, 0),
|
||||
("CRUISE_SPEED_OFFSET", 0x37c, 0),
|
||||
("EPB_STATE", 0x1c2, 0),
|
||||
("BRAKE_HOLD_ACTIVE", 0x1A4, 0),
|
||||
]
|
||||
checks = [
|
||||
# address, frequency
|
||||
(0x14a, 100),
|
||||
(0x158, 100),
|
||||
(0x17c, 100),
|
||||
(0x191, 100),
|
||||
(0x1a4, 50),
|
||||
(0x326, 10),
|
||||
(0x1b0, 50),
|
||||
(0x1d0, 50),
|
||||
(0x305, 10),
|
||||
(0x324, 10),
|
||||
(0x37c, 10),
|
||||
(0x405, 3),
|
||||
]
|
||||
signals = [
|
||||
("XMISSION_SPEED", "ENGINE_DATA", 0),
|
||||
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
||||
("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),
|
||||
("BRAKE_SWITCH", "POWERTRAIN_DATA", 0),
|
||||
("CRUISE_BUTTONS", "SCM_BUTTONS", 0),
|
||||
("ESP_DISABLED", "VSA_STATUS", 1),
|
||||
("HUD_LEAD", "ACC_HUD", 0),
|
||||
("USER_BRAKE", "VSA_STATUS", 0),
|
||||
("STEER_STATUS", "STEER_STATUS", 5),
|
||||
("GEAR_SHIFTER", "GEARBOX", 0),
|
||||
("PEDAL_GAS", "POWERTRAIN_DATA", 0),
|
||||
("CRUISE_SETTING", "SCM_BUTTONS", 0),
|
||||
("ACC_STATUS", "POWERTRAIN_DATA", 0),
|
||||
]
|
||||
|
||||
checks = [
|
||||
("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),
|
||||
("CRUISE", 10),
|
||||
("POWERTRAIN_DATA", 100),
|
||||
("VSA_STATUS", 50),
|
||||
("SCM_BUTTONS", 25),
|
||||
]
|
||||
|
||||
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.ODYSSEY:
|
||||
dbc_f = 'honda_odyssey_exl_2018_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)]
|
||||
checks += [("EPB_STATUS", 50)]
|
||||
|
||||
elif CP.carFingerprint == "ACURA ILX 2016 ACURAWATCH PLUS":
|
||||
dbc_f = 'acura_ilx_2016_can.dbc'
|
||||
signals = [
|
||||
("XMISSION_SPEED", 0x158, 0),
|
||||
("WHEEL_SPEED_FL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_FR", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RR", 0x1d0, 0),
|
||||
("STEER_ANGLE", 0x156, 0),
|
||||
("STEER_ANGLE_RATE", 0x156, 0),
|
||||
("STEER_TORQUE_SENSOR", 0x18f, 0),
|
||||
("GEAR", 0x1a3, 0),
|
||||
("WHEELS_MOVING", 0x1b0, 1),
|
||||
("DOOR_OPEN_FL", 0x405, 1),
|
||||
("DOOR_OPEN_FR", 0x405, 1),
|
||||
("DOOR_OPEN_RL", 0x405, 1),
|
||||
("DOOR_OPEN_RR", 0x405, 1),
|
||||
("CRUISE_SPEED_PCM", 0x324, 0),
|
||||
("SEATBELT_DRIVER_LAMP", 0x305, 1),
|
||||
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
|
||||
("BRAKE_PRESSED", 0x17c, 0),
|
||||
("BRAKE_SWITCH", 0x17c, 0),
|
||||
("CAR_GAS", 0x130, 0),
|
||||
("CRUISE_BUTTONS", 0x1a6, 0),
|
||||
("ESP_DISABLED", 0x1a4, 1),
|
||||
("HUD_LEAD", 0x30c, 0),
|
||||
("USER_BRAKE", 0x1a4, 0),
|
||||
("STEER_STATUS", 0x18f, 5),
|
||||
("BRAKE_ERROR_1", 0x1b0, 1),
|
||||
("BRAKE_ERROR_2", 0x1b0, 1),
|
||||
("GEAR_SHIFTER", 0x1a3, 0),
|
||||
("MAIN_ON", 0x1a6, 0),
|
||||
("ACC_STATUS", 0x17c, 0),
|
||||
("PEDAL_GAS", 0x17c, 0),
|
||||
("CRUISE_SETTING", 0x1a6, 0),
|
||||
("LEFT_BLINKER", 0x294, 0),
|
||||
("RIGHT_BLINKER", 0x294, 0),
|
||||
("CRUISE_SPEED_OFFSET", 0x37c, 0)
|
||||
]
|
||||
checks = [
|
||||
(0x156, 100),
|
||||
(0x158, 100),
|
||||
(0x17c, 100),
|
||||
(0x1a3, 50),
|
||||
(0x1a4, 50),
|
||||
(0x1a6, 50),
|
||||
(0x1b0, 50),
|
||||
(0x1d0, 50),
|
||||
(0x305, 10),
|
||||
(0x324, 10),
|
||||
(0x37c, 10),
|
||||
(0x405, 3),
|
||||
]
|
||||
elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING":
|
||||
dbc_f = 'honda_accord_touring_2016_can.dbc'
|
||||
signals = [
|
||||
("XMISSION_SPEED", 0x158, 0),
|
||||
("WHEEL_SPEED_FL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_FR", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RR", 0x1d0, 0),
|
||||
("STEER_ANGLE", 0x156, 0),
|
||||
("STEER_ANGLE_RATE", 0x156, 0),
|
||||
#("STEER_TORQUE_SENSOR", 0x18f, 0),
|
||||
("GEAR", 0x191, 0),
|
||||
("WHEELS_MOVING", 0x1b0, 1),
|
||||
("DOOR_OPEN_FL", 0x405, 1),
|
||||
("DOOR_OPEN_FR", 0x405, 1),
|
||||
("DOOR_OPEN_RL", 0x405, 1),
|
||||
("DOOR_OPEN_RR", 0x405, 1),
|
||||
("CRUISE_SPEED_PCM", 0x324, 0),
|
||||
("SEATBELT_DRIVER_LAMP", 0x305, 1),
|
||||
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
|
||||
("BRAKE_PRESSED", 0x17c, 0),
|
||||
("BRAKE_SWITCH", 0x17c, 0),
|
||||
#("CAR_GAS", 0x130, 0),
|
||||
("PEDAL_GAS", 0x17C, 0),
|
||||
("CRUISE_BUTTONS", 0x1a6, 0),
|
||||
("ESP_DISABLED", 0x1a4, 1),
|
||||
("HUD_LEAD", 0x30c, 0),
|
||||
("USER_BRAKE", 0x1a4, 0),
|
||||
#("STEER_STATUS", 0x18f, 5),
|
||||
("BRAKE_ERROR_1", 0x1b0, 1),
|
||||
("BRAKE_ERROR_2", 0x1b0, 1),
|
||||
("GEAR_SHIFTER", 0x191, 0),
|
||||
("MAIN_ON", 0x1a6, 0),
|
||||
("ACC_STATUS", 0x17c, 0),
|
||||
("PEDAL_GAS", 0x17c, 0),
|
||||
("CRUISE_SETTING", 0x1a6, 0),
|
||||
("LEFT_BLINKER", 0x294, 0),
|
||||
("RIGHT_BLINKER", 0x294, 0),
|
||||
]
|
||||
checks = [
|
||||
(0x156, 100),
|
||||
(0x158, 100),
|
||||
(0x17c, 100),
|
||||
(0x191, 100),
|
||||
(0x1a4, 50),
|
||||
(0x1a6, 50),
|
||||
(0x1b0, 50),
|
||||
(0x1d0, 50),
|
||||
(0x305, 10),
|
||||
(0x324, 10),
|
||||
(0x405, 3),
|
||||
]
|
||||
elif CP.carFingerprint == "HONDA CR-V 2016 TOURING":
|
||||
dbc_f = 'honda_crv_touring_2016_can.dbc'
|
||||
signals = [
|
||||
("XMISSION_SPEED", 0x158, 0),
|
||||
("WHEEL_SPEED_FL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_FR", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RR", 0x1d0, 0),
|
||||
("STEER_ANGLE", 0x156, 0),
|
||||
("STEER_ANGLE_RATE", 0x156, 0),
|
||||
("STEER_TORQUE_SENSOR", 0x18f, 0),
|
||||
("GEAR", 0x191, 0),
|
||||
("WHEELS_MOVING", 0x1b0, 1),
|
||||
("DOOR_OPEN_FL", 0x405, 1),
|
||||
("DOOR_OPEN_FR", 0x405, 1),
|
||||
("DOOR_OPEN_RL", 0x405, 1),
|
||||
("DOOR_OPEN_RR", 0x405, 1),
|
||||
("CRUISE_SPEED_PCM", 0x324, 0),
|
||||
("SEATBELT_DRIVER_LAMP", 0x305, 1),
|
||||
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
|
||||
("BRAKE_PRESSED", 0x17c, 0),
|
||||
("BRAKE_SWITCH", 0x17c, 0),
|
||||
#("CAR_GAS", 0x130, 0),
|
||||
("CRUISE_BUTTONS", 0x1a6, 0),
|
||||
("ESP_DISABLED", 0x1a4, 1),
|
||||
("HUD_LEAD", 0x30c, 0),
|
||||
("USER_BRAKE", 0x1a4, 0),
|
||||
("STEER_STATUS", 0x18f, 5),
|
||||
("BRAKE_ERROR_1", 0x1b0, 1),
|
||||
("BRAKE_ERROR_2", 0x1b0, 1),
|
||||
("GEAR_SHIFTER", 0x191, 0),
|
||||
("MAIN_ON", 0x1a6, 0),
|
||||
("ACC_STATUS", 0x17c, 0),
|
||||
("PEDAL_GAS", 0x17c, 0),
|
||||
("CRUISE_SETTING", 0x1a6, 0),
|
||||
("LEFT_BLINKER", 0x294, 0),
|
||||
("RIGHT_BLINKER", 0x294, 0),
|
||||
]
|
||||
checks = [
|
||||
(0x156, 100),
|
||||
(0x158, 100),
|
||||
(0x17c, 100),
|
||||
(0x191, 100),
|
||||
(0x1a4, 50),
|
||||
(0x1a6, 50),
|
||||
(0x1b0, 50),
|
||||
(0x1d0, 50),
|
||||
(0x305, 10),
|
||||
(0x324, 10),
|
||||
(0x405, 3),
|
||||
]
|
||||
elif CP.carFingerprint == "HONDA ODYSSEY 2018 EX-L":
|
||||
dbc_f = 'honda_odyssey_exl_2018.dbc'
|
||||
signals = [
|
||||
("XMISSION_SPEED", 0x158, 0),
|
||||
("WHEEL_SPEED_FL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_FR", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RR", 0x1d0, 0),
|
||||
("STEER_ANGLE", 0x156, 0),
|
||||
("STEER_ANGLE_RATE", 0x156, 0),
|
||||
("STEER_TORQUE_SENSOR", 0x18f, 0),
|
||||
("GEAR", 0x1a3, 0),
|
||||
("WHEELS_MOVING", 0x1b0, 1),
|
||||
("DOOR_OPEN_FL", 0x405, 1),
|
||||
("DOOR_OPEN_FR", 0x405, 1),
|
||||
("DOOR_OPEN_RL", 0x405, 1),
|
||||
("DOOR_OPEN_RR", 0x405, 1),
|
||||
("CRUISE_SPEED_PCM", 0x324, 0),
|
||||
("SEATBELT_DRIVER_LAMP", 0x305, 1),
|
||||
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
|
||||
("BRAKE_PRESSED", 0x17c, 0),
|
||||
("BRAKE_SWITCH", 0x17c, 0),
|
||||
("CRUISE_BUTTONS", 0x296, 0),
|
||||
("ESP_DISABLED", 0x1a4, 1),
|
||||
("HUD_LEAD", 0x30c, 0),
|
||||
("USER_BRAKE", 0x1a4, 0),
|
||||
("STEER_STATUS", 0x18f, 5),
|
||||
("BRAKE_ERROR_1", 0x1b0, 1),
|
||||
("BRAKE_ERROR_2", 0x1b0, 1),
|
||||
("GEAR_SHIFTER", 0x1a3, 0),
|
||||
("MAIN_ON", 0x326, 0),
|
||||
("ACC_STATUS", 0x17c, 0),
|
||||
("PEDAL_GAS", 0x17c, 0),
|
||||
("CRUISE_SETTING", 0x296, 0),
|
||||
("LEFT_BLINKER", 0x326, 0),
|
||||
("RIGHT_BLINKER", 0x326, 0),
|
||||
("CRUISE_SPEED_OFFSET", 0x37c, 0),
|
||||
("EPB_STATE", 0x1c2, 0),
|
||||
("BRAKE_HOLD_ACTIVE", 0x1a4, 0),
|
||||
]
|
||||
checks = [
|
||||
(0x156, 100),
|
||||
(0x158, 100),
|
||||
(0x17c, 100),
|
||||
(0x1a3, 50),
|
||||
(0x1a4, 50),
|
||||
(0x1b0, 50),
|
||||
(0x1c2, 50),
|
||||
(0x1d0, 50),
|
||||
(0x296, 25),
|
||||
(0x305, 10),
|
||||
(0x324, 10),
|
||||
(0x326, 10),
|
||||
(0x37c, 10),
|
||||
(0x405, 3),
|
||||
]
|
||||
# add gas interceptor reading if we are using it
|
||||
if CP.enableGas:
|
||||
signals.append(("INTERCEPTOR_GAS", 0x201, 0))
|
||||
checks.append((0x201, 50))
|
||||
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
|
||||
checks.append(("GAS_SENSOR", 50))
|
||||
|
||||
return dbc_f, 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)
|
||||
|
||||
|
||||
class CarState(object):
|
||||
def __init__(self, CP):
|
||||
self.acura = False
|
||||
self.civic = False
|
||||
self.accord = False
|
||||
self.crv = False
|
||||
self.odyssey = False
|
||||
if CP.carFingerprint == "HONDA CIVIC 2016 TOURING":
|
||||
self.civic = True
|
||||
elif CP.carFingerprint == "ACURA ILX 2016 ACURAWATCH PLUS":
|
||||
self.acura = True
|
||||
elif CP.carFingerprint == "HONDA ACCORD 2016 TOURING":
|
||||
self.accord = True
|
||||
elif CP.carFingerprint == "HONDA CR-V 2016 TOURING":
|
||||
self.crv = True
|
||||
elif CP.carFingerprint == "HONDA ODYSSEY 2018 EX-L":
|
||||
self.odyssey = True
|
||||
else:
|
||||
raise ValueError("unsupported car %s" % CP.carFingerprint)
|
||||
|
||||
self.brake_only = CP.enableCruise
|
||||
self.CP = CP
|
||||
|
||||
@@ -364,16 +154,13 @@ class CarState(object):
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
self.v_ego_x = np.matrix([[0.0], [0.0]])
|
||||
self.v_ego_A = np.matrix([[1.0, dt], [0.0, 1.0]])
|
||||
self.v_ego_C = np.matrix([1.0, 0.0])
|
||||
self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
self.v_ego_R = 1e3
|
||||
# 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
|
||||
# import control
|
||||
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
|
||||
# self.v_ego_K = np.transpose(K)
|
||||
self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
|
||||
|
||||
def update(self, cp):
|
||||
|
||||
@@ -381,7 +168,7 @@ class CarState(object):
|
||||
self.can_valid = cp.can_valid
|
||||
|
||||
# car params
|
||||
v_weight_v = [0., 1. ] # don't trust smooth speed at low values to avoid premature zero snapping
|
||||
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping
|
||||
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
|
||||
|
||||
# update prevs, update must run once per loop
|
||||
@@ -393,149 +180,90 @@ class CarState(object):
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
# ******************* parse out can *******************
|
||||
self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'],
|
||||
cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = not cp.vl[0x305]['SEATBELT_DRIVER_LAMP'] and cp.vl[0x305]['SEATBELT_DRIVER_LATCHED']
|
||||
# error 2 = temporary
|
||||
# error 4 = temporary, hit a bump
|
||||
# error 5 (permanent)
|
||||
# error 6 = temporary
|
||||
# error 7 (permanent)
|
||||
#self.steer_error = cp.vl[0x18F]['STEER_STATUS'] in [5,7]
|
||||
# whitelist instead of blacklist, safer at the expense of disengages
|
||||
if self.accord:
|
||||
self.steer_error = False
|
||||
self.steer_not_allowed = False
|
||||
else:
|
||||
self.steer_error = cp.vl[0x18F]['STEER_STATUS'] not in [0,2,4,6]
|
||||
self.steer_not_allowed = cp.vl[0x18F]['STEER_STATUS'] != 0
|
||||
self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
|
||||
self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
|
||||
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 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, 4, 6]
|
||||
self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] != 0
|
||||
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']
|
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
self.v_wheel_fl = cp.vl[0x1D0]['WHEEL_SPEED_FL']
|
||||
self.v_wheel_fr = cp.vl[0x1D0]['WHEEL_SPEED_FR']
|
||||
self.v_wheel_rl = cp.vl[0x1D0]['WHEEL_SPEED_RL']
|
||||
self.v_wheel_rr = cp.vl[0x1D0]['WHEEL_SPEED_RR']
|
||||
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
|
||||
self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
|
||||
|
||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||
self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
|
||||
speed = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
|
||||
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
|
||||
|
||||
if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_x = np.matrix([[speed], [0.0]])
|
||||
self.v_ego_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, speed)
|
||||
|
||||
self.v_ego_raw = speed
|
||||
self.v_ego = float(self.v_ego_x[0])
|
||||
self.a_ego = float(self.v_ego_x[1])
|
||||
v_ego_x = self.v_ego_kf.update(speed)
|
||||
self.v_ego = float(v_ego_x[0])
|
||||
self.a_ego = float(v_ego_x[1])
|
||||
|
||||
# this is a hack for the interceptor. This is now only used in the simulation
|
||||
# TODO: Replace tests by toyota so this can go away
|
||||
if self.CP.enableGas:
|
||||
# this is a hack
|
||||
self.user_gas = cp.vl[0x201]['INTERCEPTOR_GAS']
|
||||
self.user_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
|
||||
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
|
||||
#print self.user_gas, self.user_gas_pressed
|
||||
if self.civic:
|
||||
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
|
||||
self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
|
||||
self.angle_steers_rate = cp.vl[0x14A]['STEER_ANGLE_RATE']
|
||||
self.gear = 0 # TODO: civic has CVT... needs rev engineering
|
||||
self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
|
||||
self.main_on = cp.vl[0x326]['MAIN_ON']
|
||||
self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
|
||||
self.left_blinker_on = cp.vl[0x326]['LEFT_BLINKER']
|
||||
self.right_blinker_on = cp.vl[0x326]['RIGHT_BLINKER']
|
||||
self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
|
||||
self.park_brake = cp.vl[0x1c2]['EPB_STATE'] != 0
|
||||
self.brake_hold = cp.vl[0x1A4]['BRAKE_HOLD_ACTIVE']
|
||||
elif self.accord:
|
||||
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
|
||||
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
|
||||
self.angle_steers_rate = cp.vl[0x156]['STEER_ANGLE_RATE']
|
||||
self.gear = 0 # TODO: accord has CVT... needs rev engineering
|
||||
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
|
||||
self.main_on = cp.vl[0x1A6]['MAIN_ON']
|
||||
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
|
||||
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
|
||||
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
|
||||
self.cruise_speed_offset = -0.3
|
||||
self.park_brake = 0 # TODO
|
||||
self.brake_hold = 0 # TODO
|
||||
elif self.crv:
|
||||
can_gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
|
||||
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
|
||||
self.angle_steers_rate = cp.vl[0x156]['STEER_ANGLE_RATE']
|
||||
self.gear = cp.vl[0x191]['GEAR']
|
||||
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
|
||||
self.main_on = cp.vl[0x1A6]['MAIN_ON']
|
||||
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
|
||||
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
|
||||
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
|
||||
self.cruise_speed_offset = -0.3
|
||||
self.park_brake = 0 # TODO
|
||||
self.brake_hold = 0 # TODO
|
||||
elif self.odyssey:
|
||||
can_gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
|
||||
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
|
||||
self.angle_steers_rate = cp.vl[0x156]['STEER_ANGLE_RATE']
|
||||
self.gear = cp.vl[0x1A3]['GEAR']
|
||||
self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
|
||||
self.main_on = cp.vl[0x326]['MAIN_ON']
|
||||
self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
|
||||
self.left_blinker_on = cp.vl[0x326]['LEFT_BLINKER']
|
||||
self.right_blinker_on = cp.vl[0x326]['RIGHT_BLINKER']
|
||||
self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
|
||||
self.park_brake = cp.vl[0x1c2]['EPB_STATE'] != 0
|
||||
self.brake_hold = cp.vl[0x1a4]['BRAKE_HOLD_ACTIVE']
|
||||
elif self.acura:
|
||||
can_gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
|
||||
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
|
||||
self.angle_steers_rate = cp.vl[0x156]['STEER_ANGLE_RATE']
|
||||
self.gear = cp.vl[0x1A3]['GEAR']
|
||||
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
|
||||
self.main_on = cp.vl[0x1A6]['MAIN_ON']
|
||||
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
|
||||
self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER']
|
||||
self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER']
|
||||
self.cruise_speed_offset = calc_cruise_offset(cp.vl[0x37c]['CRUISE_SPEED_OFFSET'], self.v_ego)
|
||||
self.park_brake = 0 # TODO
|
||||
self.brake_hold = 0 # TODO
|
||||
|
||||
self.gear_shifter = parse_gear_shifter(can_gear_shifter, self.acura, self.odyssey)
|
||||
can_gear_shifter = cp.vl["GEARBOX"]['GEAR_SHIFTER']
|
||||
self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR']
|
||||
self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
|
||||
self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
|
||||
|
||||
if self.accord:
|
||||
# on the accord, this doesn't seem to include cruise control
|
||||
self.car_gas = cp.vl[0x17C]['PEDAL_GAS']
|
||||
self.steer_override = False
|
||||
elif self.crv or self.odyssey:
|
||||
# like accord, crv doesn't include cruise control
|
||||
self.car_gas = cp.vl[0x17C]['PEDAL_GAS']
|
||||
self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
|
||||
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
|
||||
|
||||
self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
|
||||
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):
|
||||
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.car_gas = cp.vl[0x130]['CAR_GAS']
|
||||
self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
|
||||
self.steer_torque_driver = cp.vl[0x18F]['STEER_TORQUE_SENSOR']
|
||||
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']
|
||||
# crv doesn't include cruise control
|
||||
if self.CP.carFingerprint != CAR.CRV:
|
||||
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
|
||||
else:
|
||||
self.car_gas = self.pedal_gas
|
||||
|
||||
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[0x17C]['BRAKE_SWITCH']
|
||||
self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or \
|
||||
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[0x17C]['BRAKE_SWITCH'] != self.brake_switch_ts)
|
||||
cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts)
|
||||
self.brake_switch_prev = self.brake_switch
|
||||
self.brake_switch_ts = cp.ts[0x17C]['BRAKE_SWITCH']
|
||||
self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
|
||||
self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
|
||||
self.standstill = not cp.vl[0x1B0]['WHEELS_MOVING']
|
||||
self.v_cruise_pcm = cp.vl[0x324]['CRUISE_SPEED_PCM']
|
||||
self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS']
|
||||
self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS']
|
||||
self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
|
||||
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']
|
||||
|
||||
|
||||
# carstate standalone tester
|
||||
|
||||
@@ -2,6 +2,7 @@ import struct
|
||||
|
||||
import common.numpy_fast as np
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.fingerprints import HONDA as CAR
|
||||
|
||||
|
||||
# *** Honda specific ***
|
||||
@@ -15,16 +16,19 @@ def can_cksum(mm):
|
||||
s %= 0x10
|
||||
return s
|
||||
|
||||
|
||||
def fix(msg, addr):
|
||||
msg2 = msg[0:-1] + chr(ord(msg[-1]) | can_cksum(struct.pack("I", addr)+msg))
|
||||
return msg2
|
||||
|
||||
|
||||
def make_can_msg(addr, dat, idx, alt):
|
||||
if idx is not None:
|
||||
dat += chr(idx << 4)
|
||||
dat = fix(dat, addr)
|
||||
return [addr, 0, dat, alt]
|
||||
|
||||
|
||||
def create_brake_command(apply_brake, pcm_override, pcm_cancel_cmd, chime, idx):
|
||||
"""Creates a CAN message for the Honda DBC BRAKE_COMMAND."""
|
||||
pump_on = apply_brake > 0
|
||||
@@ -39,39 +43,17 @@ def create_brake_command(apply_brake, pcm_override, pcm_cancel_cmd, chime, idx):
|
||||
brakelights << 7) + chr(chime) + "\x00"
|
||||
return make_can_msg(0x1fa, msg, idx, 0)
|
||||
|
||||
|
||||
def create_gas_command(gas_amount, idx):
|
||||
"""Creates a CAN message for the Honda DBC GAS_COMMAND."""
|
||||
msg = struct.pack("!H", gas_amount)
|
||||
return make_can_msg(0x200, msg, idx, 0)
|
||||
|
||||
def create_accord_steering_control(apply_steer, idx):
|
||||
# TODO: doesn't work for some reason
|
||||
if apply_steer == 0:
|
||||
dat = [0, 0, 0x40, 0]
|
||||
else:
|
||||
dat = [0,0,0,0]
|
||||
rp = np.clip(apply_steer/0xF, -0xFF, 0xFF)
|
||||
if rp < 0:
|
||||
rp += 512
|
||||
dat[0] |= (rp >> 5) & 0xf
|
||||
dat[1] |= (rp) & 0x1f
|
||||
if idx == 1:
|
||||
dat[0] |= 0x20
|
||||
dat[1] |= 0x20 # always
|
||||
dat[3] = -(dat[0]+dat[1]+dat[2]) & 0x7f
|
||||
|
||||
# not first byte
|
||||
dat[1] |= 0x80
|
||||
dat[2] |= 0x80
|
||||
dat[3] |= 0x80
|
||||
dat = ''.join(map(chr, dat))
|
||||
|
||||
return [0,0,dat,8]
|
||||
|
||||
def create_steering_control(apply_steer, crv, idx):
|
||||
def create_steering_control(apply_steer, car_fingerprint, idx):
|
||||
"""Creates a CAN message for the Honda DBC STEERING_CONTROL."""
|
||||
commands = []
|
||||
if crv:
|
||||
if car_fingerprint == CAR.CRV:
|
||||
msg_0x194 = struct.pack("!h", apply_steer << 4) + ("\x80" if apply_steer != 0 else "\x00")
|
||||
commands.append(make_can_msg(0x194, msg_0x194, idx, 0))
|
||||
else:
|
||||
@@ -79,7 +61,8 @@ def create_steering_control(apply_steer, crv, idx):
|
||||
commands.append(make_can_msg(0xe4, msg_0xe4, idx, 0))
|
||||
return commands
|
||||
|
||||
def create_ui_commands(pcm_speed, hud, civic, accord, crv, odyssey, idx):
|
||||
|
||||
def create_ui_commands(pcm_speed, hud, car_fingerprint, idx):
|
||||
"""Creates an iterable of CAN messages for the UIs."""
|
||||
commands = []
|
||||
pcm_speed_real = np.clip(int(round(pcm_speed / 0.002759506)), 0,
|
||||
@@ -90,43 +73,36 @@ def create_ui_commands(pcm_speed, hud, civic, accord, crv, odyssey, idx):
|
||||
|
||||
msg_0x33d = chr(hud.X5) + chr(hud.lanes) + chr(hud.beep) + chr(hud.X8)
|
||||
commands.append(make_can_msg(0x33d, msg_0x33d, idx, 0))
|
||||
if civic or odyssey: # 2 more msgs
|
||||
if car_fingerprint in (CAR.CIVIC, CAR.ODYSSEY):
|
||||
msg_0x35e = chr(0) * 7
|
||||
commands.append(make_can_msg(0x35e, msg_0x35e, idx, 0))
|
||||
if civic or accord or odyssey:
|
||||
msg_0x39f = (
|
||||
chr(0) * 2 + chr(hud.acc_alert) + chr(0) + chr(0xff) + chr(0x7f) + chr(0)
|
||||
)
|
||||
msg_0x39f = (chr(0) * 2 + chr(hud.acc_alert) + chr(0) + chr(0xff) + chr(0x7f) + chr(0))
|
||||
commands.append(make_can_msg(0x39f, msg_0x39f, idx, 0))
|
||||
return commands
|
||||
|
||||
def create_radar_commands(v_ego, civic, accord, crv, odyssey, idx):
|
||||
|
||||
def create_radar_commands(v_ego, car_fingerprint, idx):
|
||||
"""Creates an iterable of CAN messages for the radar system."""
|
||||
commands = []
|
||||
v_ego_kph = np.clip(int(round(v_ego * CV.MS_TO_KPH)), 0, 255)
|
||||
speed = struct.pack('!B', v_ego_kph)
|
||||
|
||||
msg_0x300 = ("\xf9" + speed + "\x8a\xd0" +\
|
||||
("\x20" if idx == 0 or idx == 3 else "\x00") +\
|
||||
"\x00\x00")
|
||||
if civic:
|
||||
msg_0x300 = ("\xf9" + speed + "\x8a\xd0" +
|
||||
("\x20" if idx == 0 or idx == 3 else "\x00") +
|
||||
"\x00\x00")
|
||||
|
||||
if car_fingerprint == CAR.CIVIC:
|
||||
msg_0x301 = "\x02\x38\x44\x32\x4f\x00\x00"
|
||||
# add 8 on idx.
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx + 8, 1))
|
||||
elif accord:
|
||||
# 0300( 768)( 69) f9008ad0100000ef
|
||||
# 0301( 769)( 69) 0ed8522256000029
|
||||
msg_0x301 = "\x0e\xd8\x52\x22\x56\x00\x00"
|
||||
# add 0xc on idx? WTF is this?
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx + 0xc, 1))
|
||||
elif crv:
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx + 8, 1)) # add 8 on idx.
|
||||
elif car_fingerprint == CAR.CRV:
|
||||
msg_0x301 = "\x00\x00\x50\x02\x51\x00\x00"
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
|
||||
elif odyssey:
|
||||
elif car_fingerprint == CAR.ODYSSEY:
|
||||
msg_0x301 = "\x00\x00\x56\x02\x55\x00\x00"
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
|
||||
else:
|
||||
elif car_fingerprint == CAR.ACURA_ILX:
|
||||
msg_0x301 = "\x0f\x18\x51\x02\x5a\x00\x00"
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
|
||||
|
||||
commands.append(make_can_msg(0x301, msg_0x301, idx, 1))
|
||||
return commands
|
||||
|
||||
@@ -1,17 +1,16 @@
|
||||
#!/usr/bin/env python
|
||||
import os
|
||||
import numpy as np
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
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, get_events
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from cereal import car
|
||||
from selfdrive.services import service_list
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.car.honda.carstate import CarState, get_can_parser
|
||||
from selfdrive.car.honda.values import CruiseButtons, CM, BP, AH
|
||||
from selfdrive.controls.lib.planner import A_ACC_MAX
|
||||
from common.fingerprints import HONDA as CAR
|
||||
|
||||
try:
|
||||
from .carcontroller import CarController
|
||||
@@ -100,11 +99,7 @@ class CarInterface(object):
|
||||
self.sendcan = sendcan
|
||||
self.CC = CarController(CP.enableCamera)
|
||||
|
||||
if self.CS.accord:
|
||||
# self.accord_msg = []
|
||||
raise NotImplementedError
|
||||
|
||||
if not self.CS.civic and not self.CS.odyssey:
|
||||
if self.CS.CP.carFingerprint == CAR.ACURA_ILX:
|
||||
self.compute_gb = get_compute_gb_acura()
|
||||
else:
|
||||
self.compute_gb = compute_gb_honda
|
||||
@@ -132,7 +127,6 @@ class CarInterface(object):
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "honda"
|
||||
ret.radarName = "nidec"
|
||||
ret.carFingerprint = candidate
|
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModels.honda
|
||||
@@ -157,7 +151,7 @@ class CarInterface(object):
|
||||
tireStiffnessFront_civic = 85400
|
||||
tireStiffnessRear_civic = 90000
|
||||
|
||||
if candidate == "HONDA CIVIC 2016 TOURING":
|
||||
if candidate == CAR.CIVIC:
|
||||
stop_and_go = True
|
||||
ret.mass = mass_civic
|
||||
ret.wheelbase = wheelbase_civic
|
||||
@@ -171,7 +165,7 @@ class CarInterface(object):
|
||||
ret.longitudinalKpV = [3.6, 2.4, 1.5]
|
||||
ret.longitudinalKiBP = [0., 35.]
|
||||
ret.longitudinalKiV = [0.54, 0.36]
|
||||
elif candidate == "ACURA ILX 2016 ACURAWATCH PLUS":
|
||||
elif candidate == CAR.ACURA_ILX:
|
||||
stop_and_go = False
|
||||
ret.mass = 3095./2.205 + std_cargo
|
||||
ret.wheelbase = 2.67
|
||||
@@ -185,19 +179,7 @@ class CarInterface(object):
|
||||
ret.longitudinalKpV = [1.2, 0.8, 0.5]
|
||||
ret.longitudinalKiBP = [0., 35.]
|
||||
ret.longitudinalKiV = [0.18, 0.12]
|
||||
elif candidate == "HONDA ACCORD 2016 TOURING":
|
||||
stop_and_go = False
|
||||
ret.mass = 3580./2.205 + std_cargo
|
||||
ret.wheelbase = 2.74
|
||||
ret.centerToFront = ret.wheelbase * 0.38
|
||||
ret.steerRatio = 15.3
|
||||
ret.steerKp, ret.steerKi = 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 == "HONDA CR-V 2016 TOURING":
|
||||
elif candidate == CAR.CRV:
|
||||
stop_and_go = False
|
||||
ret.mass = 3572./2.205 + std_cargo
|
||||
ret.wheelbase = 2.62
|
||||
@@ -209,7 +191,7 @@ class CarInterface(object):
|
||||
ret.longitudinalKpV = [1.2, 0.8, 0.5]
|
||||
ret.longitudinalKiBP = [0., 35.]
|
||||
ret.longitudinalKiV = [0.18, 0.12]
|
||||
elif candidate == "HONDA ODYSSEY 2018 EX-L":
|
||||
elif candidate == CAR.ODYSSEY:
|
||||
stop_and_go = False
|
||||
ret.mass = 4354./2.205 + std_cargo
|
||||
ret.wheelbase = 3.00
|
||||
@@ -303,7 +285,7 @@ class CarInterface(object):
|
||||
ret.brake = self.CS.user_brake
|
||||
ret.brakePressed = self.CS.brake_pressed != 0
|
||||
# FIXME: read sendcan for brakelights
|
||||
brakelights_threshold = 0.02 if self.CS.civic else 0.1
|
||||
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
|
||||
ret.brakeLights = bool(self.CS.brake_switch or
|
||||
c.actuators.brake > brakelights_threshold)
|
||||
|
||||
@@ -329,6 +311,9 @@ class CarInterface(object):
|
||||
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
|
||||
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = 'leftBlinker'
|
||||
@@ -393,9 +378,9 @@ class CarInterface(object):
|
||||
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
if not ret.gearShifter == 'drive':
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.door_all_closed:
|
||||
if ret.doorOpen:
|
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.seatbelt:
|
||||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
@@ -424,7 +409,7 @@ class CarInterface(object):
|
||||
# TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
|
||||
if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.:
|
||||
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
|
||||
if not self.CS.civic and ret.vEgo < 0.001:
|
||||
if self.CS.CP.carFingerprint != CAR.CIVIC and ret.vEgo < 0.001:
|
||||
events.append(create_event('manualRestart', [ET.WARNING]))
|
||||
|
||||
cur_time = sec_since_boot()
|
||||
|
||||
@@ -47,7 +47,6 @@ class CarInterface(object):
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "mock"
|
||||
ret.radarName = "mock"
|
||||
ret.carFingerprint = candidate
|
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModels.noOutput
|
||||
|
||||
@@ -6,7 +6,8 @@ from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
|
||||
create_steer_command, create_ui_command, \
|
||||
create_ipas_steer_command, create_accel_command, \
|
||||
create_fcw_command
|
||||
from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS
|
||||
from selfdrive.car.toyota.values import ECU, STATIC_MSGS
|
||||
from common.fingerprints import TOYOTA as CAR
|
||||
|
||||
|
||||
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
|
||||
@@ -17,10 +18,7 @@ ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
|
||||
STEER_MAX = 1500
|
||||
STEER_DELTA_UP = 10 # 1.5s time to peak torque
|
||||
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
|
||||
STEER_ERROR_MAX = 500 # max delta between torque cmd and torque motor
|
||||
|
||||
STEER_IPAS_MAX = 340
|
||||
STEER_IPAS_DELTA_MAX = 3
|
||||
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
|
||||
|
||||
TARGET_IDS = [0x340, 0x341, 0x342, 0x343, 0x344, 0x345,
|
||||
0x363, 0x364, 0x365, 0x370, 0x371, 0x372,
|
||||
|
||||
@@ -1,12 +1,14 @@
|
||||
import os
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.car.toyota.values import CAR
|
||||
from common.fingerprints import TOYOTA as CAR
|
||||
from selfdrive.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
import numpy as np
|
||||
|
||||
def parse_gear_shifter(can_gear, car_fingerprint):
|
||||
|
||||
def parse_gear_shifter(can_gear, car_fingerprint):
|
||||
# TODO: Use values from DBC to parse this field
|
||||
if car_fingerprint == CAR.PRIUS:
|
||||
if can_gear == 0x0:
|
||||
return "park"
|
||||
@@ -18,7 +20,7 @@ def parse_gear_shifter(can_gear, car_fingerprint):
|
||||
return "drive"
|
||||
elif can_gear == 0x4:
|
||||
return "brake"
|
||||
elif car_fingerprint in [CAR.RAV4, CAR.RAV4H]:
|
||||
elif car_fingerprint in [CAR.RAV4, CAR.RAV4H, CAR.COROLLA]:
|
||||
if can_gear == 0x20:
|
||||
return "park"
|
||||
elif can_gear == 0x10:
|
||||
@@ -36,74 +38,53 @@ def parse_gear_shifter(can_gear, car_fingerprint):
|
||||
def get_can_parser(CP):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
if CP.carFingerprint == CAR.PRIUS:
|
||||
dbc_f = 'toyota_prius_2017_pt.dbc'
|
||||
signals = [
|
||||
("GEAR", 295, 0),
|
||||
("BRAKE_PRESSED", 550, 0),
|
||||
("GAS_PEDAL", 581, 0),
|
||||
]
|
||||
checks = [
|
||||
(550, 40),
|
||||
(581, 33)
|
||||
]
|
||||
dbc_f = 'toyota_prius_2017_pt_generated.dbc'
|
||||
elif CP.carFingerprint == CAR.RAV4H:
|
||||
dbc_f = 'toyota_rav4_hybrid_2017_pt.dbc'
|
||||
signals = [
|
||||
("GEAR", 956, 0),
|
||||
("BRAKE_PRESSED", 550, 0),
|
||||
("GAS_PEDAL", 581, 0),
|
||||
]
|
||||
checks = [
|
||||
(550, 40),
|
||||
(581, 33)
|
||||
]
|
||||
dbc_f = 'toyota_rav4_hybrid_2017_pt_generated.dbc'
|
||||
elif CP.carFingerprint == CAR.RAV4:
|
||||
dbc_f = 'toyota_rav4_2017_pt.dbc'
|
||||
signals = [
|
||||
("GEAR", 956, 0x20),
|
||||
("BRAKE_PRESSED", 548, 0),
|
||||
("GAS_PEDAL", 705, 0),
|
||||
]
|
||||
checks = [
|
||||
(548, 40),
|
||||
(705, 33)
|
||||
]
|
||||
dbc_f = 'toyota_rav4_2017_pt_generated.dbc'
|
||||
elif CP.carFingerprint == CAR.COROLLA:
|
||||
dbc_f = 'toyota_corolla_2017_pt_generated.dbc'
|
||||
|
||||
# TODO: DOORS, GAS_PEDAL, BRAKE_PRESSED for RAV4
|
||||
signals += [
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("WHEEL_SPEED_FL", 170, 0),
|
||||
("WHEEL_SPEED_FR", 170, 0),
|
||||
("WHEEL_SPEED_RL", 170, 0),
|
||||
("WHEEL_SPEED_RR", 170, 0),
|
||||
("DOOR_OPEN_FL", 1568, 1),
|
||||
("DOOR_OPEN_FR", 1568, 1),
|
||||
("DOOR_OPEN_RL", 1568, 1),
|
||||
("DOOR_OPEN_RR", 1568, 1),
|
||||
("SEATBELT_DRIVER_UNLATCHED", 1568, 1),
|
||||
("TC_DISABLED", 951, 1),
|
||||
("STEER_ANGLE", 37, 0),
|
||||
("STEER_FRACTION", 37, 0),
|
||||
("STEER_RATE", 37, 0),
|
||||
("GAS_RELEASED", 466, 0),
|
||||
("CRUISE_STATE", 466, 0),
|
||||
("MAIN_ON", 467, 0),
|
||||
("SET_SPEED", 467, 0),
|
||||
("LOW_SPEED_LOCKOUT", 467, 0),
|
||||
("STEER_TORQUE_DRIVER", 608, 0),
|
||||
("STEER_TORQUE_EPS", 608, 0),
|
||||
("TURN_SIGNALS", 1556, 3), # 3 is no blinkers
|
||||
("LKA_STATE", 610, 0),
|
||||
("BRAKE_LIGHTS_ACC", 951, 0),
|
||||
("GEAR", "GEAR_PACKET", 0),
|
||||
("BRAKE_PRESSED", "BRAKE_MODULE", 0),
|
||||
("GAS_PEDAL", "GAS_PEDAL", 0),
|
||||
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
||||
("DOOR_OPEN_FL", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_FR", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_RL", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_RR", "SEATS_DOORS", 1),
|
||||
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1),
|
||||
("TC_DISABLED", "ESP_CONTROL", 1),
|
||||
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
|
||||
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
|
||||
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
|
||||
("GAS_RELEASED", "PCM_CRUISE", 0),
|
||||
("CRUISE_STATE", "PCM_CRUISE", 0),
|
||||
("MAIN_ON", "PCM_CRUISE_2", 0),
|
||||
("SET_SPEED", "PCM_CRUISE_2", 0),
|
||||
("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0),
|
||||
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
|
||||
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
|
||||
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
|
||||
("LKA_STATE", "EPS_STATUS", 0),
|
||||
("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
|
||||
]
|
||||
|
||||
checks += [
|
||||
(170, 80),
|
||||
(37, 80),
|
||||
(466, 33),
|
||||
(467, 33),
|
||||
(608, 50),
|
||||
(610, 25),
|
||||
checks = [
|
||||
("BRAKE_MODULE", 40),
|
||||
("GAS_PEDAL", 33),
|
||||
("WHEEL_SPEEDS", 80),
|
||||
("STEER_ANGLE_SENSOR", 80),
|
||||
("PCM_CRUISE", 33),
|
||||
("PCM_CRUISE_2", 33),
|
||||
("STEER_TORQUE_SENSOR", 50),
|
||||
("EPS_STATUS", 25),
|
||||
]
|
||||
|
||||
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 0)
|
||||
@@ -121,82 +102,68 @@ class CarState(object):
|
||||
|
||||
# vEgo kalman filter
|
||||
dt = 0.01
|
||||
self.v_ego_x = np.matrix([[0.0], [0.0]])
|
||||
self.v_ego_A = np.matrix([[1.0, dt], [0.0, 1.0]])
|
||||
self.v_ego_C = np.matrix([1.0, 0.0])
|
||||
self.v_ego_Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
|
||||
self.v_ego_R = 1e3
|
||||
# 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
|
||||
# import control
|
||||
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
|
||||
# self.v_ego_K = np.transpose(K)
|
||||
self.v_ego_K = np.matrix([[0.12287673], [0.29666309]])
|
||||
|
||||
def update(self, cp):
|
||||
|
||||
# copy can_valid
|
||||
self.can_valid = cp.can_valid
|
||||
|
||||
if self.car_fingerprint == CAR.PRIUS:
|
||||
can_gear = cp.vl[295]['GEAR']
|
||||
self.brake_pressed = cp.vl[550]['BRAKE_PRESSED']
|
||||
self.pedal_gas = cp.vl[581]['GAS_PEDAL']
|
||||
elif self.car_fingerprint == CAR.RAV4H:
|
||||
can_gear = cp.vl[956]['GEAR']
|
||||
self.brake_pressed = cp.vl[550]['BRAKE_PRESSED']
|
||||
self.pedal_gas = cp.vl[581]['GAS_PEDAL']
|
||||
elif self.car_fingerprint == CAR.RAV4:
|
||||
can_gear = cp.vl[956]['GEAR']
|
||||
self.brake_pressed = cp.vl[548]['BRAKE_PRESSED']
|
||||
self.pedal_gas = cp.vl[705]['GAS_PEDAL']
|
||||
|
||||
# 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
|
||||
|
||||
# ******************* parse out can *******************
|
||||
self.door_all_closed = not any([cp.vl[1568]['DOOR_OPEN_FL'], cp.vl[1568]['DOOR_OPEN_FR'],
|
||||
cp.vl[1568]['DOOR_OPEN_RL'], cp.vl[1568]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = not cp.vl[1568]['SEATBELT_DRIVER_UNLATCHED']
|
||||
# whitelist instead of blacklist, safer at the expense of disengages
|
||||
self.door_all_closed = not any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
|
||||
|
||||
self.steer_error = False
|
||||
self.brake_error = 0
|
||||
self.esp_disabled = cp.vl[951]['TC_DISABLED']
|
||||
|
||||
can_gear = cp.vl["GEAR_PACKET"]['GEAR']
|
||||
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
|
||||
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
|
||||
self.car_gas = self.pedal_gas
|
||||
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
|
||||
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
self.v_wheel_fl = cp.vl[170]['WHEEL_SPEED_FL']
|
||||
self.v_wheel_fr = cp.vl[170]['WHEEL_SPEED_FR']
|
||||
self.v_wheel_rl = cp.vl[170]['WHEEL_SPEED_RL']
|
||||
self.v_wheel_rr = cp.vl[170]['WHEEL_SPEED_RR']
|
||||
self.v_wheel = (
|
||||
cp.vl[170]['WHEEL_SPEED_FL'] + cp.vl[170]['WHEEL_SPEED_FR'] +
|
||||
cp.vl[170]['WHEEL_SPEED_RL'] + cp.vl[170]['WHEEL_SPEED_RR']) / 4. * CV.KPH_TO_MS
|
||||
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
|
||||
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_x = np.dot((self.v_ego_A - np.dot(self.v_ego_K, self.v_ego_C)), self.v_ego_x) + np.dot(self.v_ego_K, self.v_wheel)
|
||||
|
||||
self.v_ego_raw = self.v_wheel
|
||||
self.v_ego = float(self.v_ego_x[0])
|
||||
self.a_ego = float(self.v_ego_x[1])
|
||||
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[37]['STEER_ANGLE'] + cp.vl[37]['STEER_FRACTION']
|
||||
self.angle_steers_rate = cp.vl[37]['STEER_RATE']
|
||||
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
|
||||
self.gear_shifter = parse_gear_shifter(can_gear, self.car_fingerprint)
|
||||
self.main_on = cp.vl[467]['MAIN_ON']
|
||||
self.left_blinker_on = cp.vl[1556]['TURN_SIGNALS'] == 1
|
||||
self.right_blinker_on = cp.vl[1556]['TURN_SIGNALS'] == 2
|
||||
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
|
||||
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
|
||||
# we could use the override bit from dbc, but it's triggered at too high torque values
|
||||
self.steer_override = abs(cp.vl[608]['STEER_TORQUE_DRIVER']) > 100
|
||||
self.steer_error = cp.vl[610]['LKA_STATE'] == 50
|
||||
self.steer_torque_driver = cp.vl[608]['STEER_TORQUE_DRIVER']
|
||||
self.steer_torque_motor = cp.vl[608]['STEER_TORQUE_EPS']
|
||||
self.steer_override = abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100
|
||||
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] == 50
|
||||
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
|
||||
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
|
||||
|
||||
self.user_brake = 0
|
||||
self.v_cruise_pcm = cp.vl[467]['SET_SPEED']
|
||||
self.pcm_acc_status = cp.vl[466]['CRUISE_STATE']
|
||||
self.car_gas = self.pedal_gas
|
||||
self.gas_pressed = not cp.vl[466]['GAS_RELEASED']
|
||||
self.low_speed_lockout = cp.vl[467]['LOW_SPEED_LOCKOUT'] == 2
|
||||
self.brake_lights = bool(cp.vl[951]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
|
||||
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
|
||||
self.gas_pressed = not cp.vl["PCM_CRUISE"]['GAS_RELEASED']
|
||||
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
|
||||
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
|
||||
|
||||
@@ -9,12 +9,15 @@ import selfdrive.messaging as messaging
|
||||
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 CAR, ECU, check_ecu_msgs
|
||||
from selfdrive.car.toyota.values import ECU, check_ecu_msgs
|
||||
from common.fingerprints import TOYOTA as CAR
|
||||
|
||||
try:
|
||||
from selfdrive.car.toyota.carcontroller import CarController
|
||||
except ImportError:
|
||||
CarController = None
|
||||
|
||||
|
||||
class CarInterface(object):
|
||||
def __init__(self, CP, sendcan=None):
|
||||
self.CP = CP
|
||||
@@ -53,7 +56,6 @@ class CarInterface(object):
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "toyota"
|
||||
ret.radarName = "toyota"
|
||||
ret.carFingerprint = candidate
|
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModels.toyota
|
||||
@@ -74,12 +76,30 @@ class CarInterface(object):
|
||||
tireStiffnessFront_civic = 85400
|
||||
tireStiffnessRear_civic = 90000
|
||||
|
||||
ret.mass = 3045./2.205 + std_cargo
|
||||
ret.wheelbase = 2.70 if candidate == CAR.PRIUS else 2.65
|
||||
if candidate == CAR.PRIUS:
|
||||
ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
ret.wheelbase = 2.70
|
||||
ret.steerRatio = 14.5 #TODO: find exact value for Prius
|
||||
ret.mass = 3045./2.205 + std_cargo
|
||||
elif candidate in [CAR.RAV4, CAR.RAV4H]:
|
||||
ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
ret.wheelbase = 2.65
|
||||
ret.steerRatio = 14.5 # Rav4 2017
|
||||
ret.mass = 3650./2.205 + std_cargo # mean between normal and hybrid
|
||||
elif candidate == CAR.COROLLA:
|
||||
ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
ret.wheelbase = 2.70
|
||||
ret.steerRatio = 17.8
|
||||
ret.mass = 2860./2.205 + std_cargo # mean between normal and hybrid
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
ret.steerRatio = 14.5 #Rav4 2017, TODO: find exact value for Prius
|
||||
ret.steerKp, ret.steerKi = 0.6, 0.05
|
||||
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
|
||||
|
||||
if candidate == CAR.COROLLA:
|
||||
ret.steerKp, ret.steerKi = 0.2, 0.05
|
||||
ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594
|
||||
else:
|
||||
ret.steerKp, ret.steerKi = 0.6, 0.05
|
||||
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
|
||||
|
||||
ret.longPidDeadzoneBP = [0., 9.]
|
||||
ret.longPidDeadzoneV = [0., .15]
|
||||
@@ -88,7 +108,7 @@ class CarInterface(object):
|
||||
# to a negative value, so it won't matter.
|
||||
if candidate in [CAR.PRIUS, CAR.RAV4H]: # rav4 hybrid can do stop and go
|
||||
ret.minEnableSpeed = -1.
|
||||
elif candidate == CAR.RAV4: # TODO: hack ICE Rav4 to do stop and go
|
||||
elif candidate in [CAR.RAV4, CAR.COROLLA]: # TODO: hack ICE to do stop and go
|
||||
ret.minEnableSpeed = 19. * CV.MPH_TO_MS
|
||||
|
||||
centerToRear = ret.wheelbase - ret.centerToFront
|
||||
@@ -134,9 +154,9 @@ class CarInterface(object):
|
||||
ret.longitudinalKiBP = [0., 35.]
|
||||
ret.longitudinalKiV = [0.54, 0.36]
|
||||
|
||||
if candidate == CAR.PRIUS:
|
||||
if candidate in [CAR.PRIUS]:
|
||||
ret.steerRateCost = 2.
|
||||
elif candidate in [CAR.RAV4, CAR.RAV4H]:
|
||||
elif candidate in [CAR.RAV4, CAR.RAV4H, CAR.COROLLA]:
|
||||
ret.steerRateCost = 1.
|
||||
|
||||
return ret
|
||||
@@ -194,7 +214,7 @@ class CarInterface(object):
|
||||
# receiving any special command
|
||||
ret.cruiseState.standstill = False
|
||||
else:
|
||||
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
|
||||
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
|
||||
|
||||
# TODO: button presses
|
||||
buttonEvents = []
|
||||
@@ -215,6 +235,9 @@ class CarInterface(object):
|
||||
ret.leftBlinker = bool(self.CS.left_blinker_on)
|
||||
ret.rightBlinker = bool(self.CS.right_blinker_on)
|
||||
|
||||
ret.doorOpen = not self.CS.door_all_closed
|
||||
ret.seatbeltUnlatched = not self.CS.seatbelt
|
||||
|
||||
# events
|
||||
events = []
|
||||
if not self.CS.can_valid:
|
||||
@@ -225,9 +248,9 @@ class CarInterface(object):
|
||||
self.can_invalid_count = 0
|
||||
if not ret.gearShifter == 'drive' and self.CP.enableDsu:
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.door_all_closed:
|
||||
if ret.doorOpen:
|
||||
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if not self.CS.seatbelt:
|
||||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if self.CS.esp_disabled and self.CP.enableDsu:
|
||||
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
@@ -1,64 +1,61 @@
|
||||
class CAR:
|
||||
PRIUS = "TOYOTA PRIUS 2017"
|
||||
RAV4H = "TOYOTA RAV4 2017 HYBRID"
|
||||
RAV4 = "TOYOTA RAV4 2017"
|
||||
from common.fingerprints import TOYOTA as CAR
|
||||
|
||||
class ECU:
|
||||
CAM = 0 # camera
|
||||
DSU = 1 # driving support unit
|
||||
APGS = 2 # advanced parking guidance system
|
||||
|
||||
|
||||
class ECU:
|
||||
CAM = 0 # camera
|
||||
DSU = 1 # driving support unit
|
||||
APGS = 2 # advanced parking guidance system
|
||||
# addr: (ecu, cars, bus, 1/freq*100, vl)
|
||||
STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 1, 2, '\x00\x00\x00\x46'),
|
||||
0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 1, 3, '\xf4\x01\x90\x83\x00\x37'),
|
||||
|
||||
|
||||
# addr, [ecu, bus, 1/freq*100, vl]
|
||||
STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 2, '\x00\x00\x00\x46'),
|
||||
0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 3, '\xf4\x01\x90\x83\x00\x37'),
|
||||
|
||||
0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
|
||||
0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'),
|
||||
0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
|
||||
0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'),
|
||||
0x2E6: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
|
||||
0x2E7: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
|
||||
|
||||
0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'),
|
||||
0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
|
||||
0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'),
|
||||
0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
|
||||
|
||||
0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
|
||||
0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'),
|
||||
0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
|
||||
0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'),
|
||||
|
||||
0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x33E: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'),
|
||||
0x365: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'),
|
||||
0x365: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'),
|
||||
0x365: (ECU.DSU, (CAR.RAV4, CAR.COROLLA), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'),
|
||||
0x366: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'),
|
||||
0x366: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'),
|
||||
0x366: (ECU.DSU, (CAR.RAV4, CAR.COROLLA), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'),
|
||||
|
||||
0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 40, '\x06\x00'),
|
||||
0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 40, '\x06\x00'),
|
||||
|
||||
0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
|
||||
0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
|
||||
0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
|
||||
0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
|
||||
0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
|
||||
0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
|
||||
0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
|
||||
0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
|
||||
0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 100, '\x20\x20\xAD'),
|
||||
0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'),
|
||||
0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x466: (ECU.CAM, (CAR.COROLLA), 1, 100, '\x24\x20\xB1'),
|
||||
0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'),
|
||||
0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4, CAR.COROLLA), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
|
||||
0x470: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 1, 100, '\x00\x00\x02\x7a'),
|
||||
}
|
||||
|
||||
|
||||
def check_ecu_msgs(fingerprint, candidate, ecu):
|
||||
# return True if fingerprint contains messages normally sent by a given ecu
|
||||
ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and
|
||||
candidate in STATIC_MSGS[x][1] and
|
||||
ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and
|
||||
candidate in STATIC_MSGS[x][1] and
|
||||
STATIC_MSGS[x][2] == 0)]
|
||||
|
||||
return any(msg for msg in fingerprint if msg in ecu_msgs)
|
||||
|
||||
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "0.4.0.2-openpilot"
|
||||
#define COMMA_VERSION "0.4.1-openpilot"
|
||||
|
||||
@@ -3,6 +3,7 @@ import os
|
||||
import zmq
|
||||
import numpy as np
|
||||
import numpy.matlib
|
||||
import importlib
|
||||
from collections import defaultdict
|
||||
from fastcluster import linkage_vector
|
||||
import selfdrive.messaging as messaging
|
||||
@@ -49,14 +50,13 @@ def radard_thread(gctx=None):
|
||||
# wait for stats about the car to come in from controls
|
||||
cloudlog.info("radard is waiting for CarParams")
|
||||
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
|
||||
mocked= CP.radarName == "mock"
|
||||
mocked = CP.carName == "mock"
|
||||
VM = VehicleModel(CP)
|
||||
cloudlog.info("radard got CarParams")
|
||||
|
||||
# import the radar from the fingerprint
|
||||
cloudlog.info("radard is importing %s", CP.radarName)
|
||||
exec('from selfdrive.radar.'+CP.radarName+'.interface import RadarInterface')
|
||||
|
||||
cloudlog.info("radard is importing %s", CP.carName)
|
||||
RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
|
||||
context = zmq.Context()
|
||||
|
||||
# *** subscribe to features and model from visiond
|
||||
|
||||
@@ -16,6 +16,7 @@ if __name__ == "__main__":
|
||||
parser.add_argument('--pipe', action='store_true')
|
||||
parser.add_argument('--raw', action='store_true')
|
||||
parser.add_argument('--json', action='store_true')
|
||||
parser.add_argument('--dump-json', action='store_true')
|
||||
parser.add_argument('--addr', default='127.0.0.1')
|
||||
parser.add_argument("socket", type=str, nargs='*', help="socket name")
|
||||
args = parser.parse_args()
|
||||
@@ -41,6 +42,8 @@ if __name__ == "__main__":
|
||||
hexdump(sock.recv())
|
||||
elif args.json:
|
||||
print(json.loads(sock.recv()))
|
||||
elif args.dump_json:
|
||||
print json.dumps(messaging.recv_one(sock).to_dict())
|
||||
else:
|
||||
print messaging.recv_one(sock)
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -53,6 +53,12 @@ trimbleGnss: [8041, true]
|
||||
ubloxRaw: [8042, true]
|
||||
gpsPlannerPoints: [8043, true]
|
||||
gpsPlannerPlan: [8044, true]
|
||||
applanixRaw: [8046, true]
|
||||
orbLocation: [8047, true]
|
||||
trafficSigns: [8048, true]
|
||||
liveLocationTiming: [8049, true]
|
||||
orbslamCorrection: [8050, true]
|
||||
liveLocationCorrected: [8051, true]
|
||||
|
||||
testModel: [8040, false]
|
||||
testLiveLocation: [8045, false]
|
||||
|
||||
@@ -8,11 +8,12 @@ import numpy as np
|
||||
from opendbc import DBC_PATH
|
||||
|
||||
from common.realtime import Ratekeeper
|
||||
|
||||
from selfdrive.config import Conversions as CV
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.config import CruiseButtons
|
||||
from selfdrive.car.honda.hondacan import fix
|
||||
from common.fingerprints import HONDA as 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
|
||||
|
||||
@@ -21,10 +22,10 @@ from selfdrive.car.honda.interface import CarInterface
|
||||
|
||||
from cereal import car
|
||||
from common.dbc import dbc
|
||||
honda = dbc(os.path.join(DBC_PATH, "honda_civic_touring_2016_can.dbc"))
|
||||
honda = dbc(os.path.join(DBC_PATH, "honda_civic_touring_2016_can_generated.dbc"))
|
||||
|
||||
# Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor
|
||||
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {0x201})
|
||||
CP = CarInterface.get_params(CAR.CIVIC, {0x201})
|
||||
|
||||
|
||||
def car_plant(pos, speed, grade, gas, brake):
|
||||
@@ -66,7 +67,7 @@ def car_plant(pos, speed, grade, gas, brake):
|
||||
return speed, acceleration
|
||||
|
||||
def get_car_can_parser():
|
||||
dbc_f = 'honda_civic_touring_2016_can.dbc'
|
||||
dbc_f = 'honda_civic_touring_2016_can_generated.dbc'
|
||||
signals = [
|
||||
("STEER_TORQUE", 0xe4, 0),
|
||||
("STEER_TORQUE_REQUEST", 0xe4, 0),
|
||||
@@ -92,7 +93,6 @@ class Plant(object):
|
||||
|
||||
def __init__(self, lead_relevancy=False, rate=100, speed=0.0, distance_lead=2.0):
|
||||
self.rate = rate
|
||||
self.civic = True
|
||||
self.brake_only = False
|
||||
|
||||
if not Plant.messaging_initialized:
|
||||
@@ -143,7 +143,7 @@ class Plant(object):
|
||||
if speed<0.3:
|
||||
return 0
|
||||
else:
|
||||
return speed
|
||||
return speed * CV.MS_TO_KPH
|
||||
|
||||
def current_time(self):
|
||||
return float(self.rk.frame) / self.rate
|
||||
@@ -217,15 +217,32 @@ class Plant(object):
|
||||
|
||||
# ******** publish the car ********
|
||||
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, self.gear_choice, speed!=0,
|
||||
0, 0, 0, 0,
|
||||
self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed, 0.,
|
||||
self.user_gas, cruise_buttons, self.esp_disabled, 0,
|
||||
self.user_brake, self.steer_error, self.brake_error,
|
||||
self.brake_error, self.gear_shifter, self.main_on, self.acc_status,
|
||||
self.pedal_gas, self.cruise_setting,
|
||||
# append one more zero for gas interceptor
|
||||
0,0,0,0,0,0]
|
||||
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,
|
||||
self.esp_disabled,
|
||||
0, # HUD lead
|
||||
self.user_brake,
|
||||
self.steer_error,
|
||||
self.gear_shifter,
|
||||
self.pedal_gas,
|
||||
self.cruise_setting,
|
||||
self.acc_status,
|
||||
self.user_gas,
|
||||
self.main_on,
|
||||
0, # EPB State
|
||||
0, # Brake hold
|
||||
0, # Interceptor feedback
|
||||
# 0,
|
||||
]
|
||||
|
||||
# TODO: publish each message at proper frequency
|
||||
can_msgs = []
|
||||
@@ -238,6 +255,7 @@ class Plant(object):
|
||||
if "COUNTER" in honda.get_signals(msg):
|
||||
msg_struct["COUNTER"] = self.rk.frame % 4
|
||||
|
||||
msg = honda.lookup_msg_id(msg)
|
||||
msg_data = honda.encode(msg, msg_struct)
|
||||
|
||||
if "CHECKSUM" in honda.get_signals(msg):
|
||||
@@ -302,4 +320,3 @@ def plant_thread(rate=100):
|
||||
|
||||
if __name__ == "__main__":
|
||||
plant_thread()
|
||||
|
||||
|
||||
Binary file not shown.
Reference in New Issue
Block a user