diff --git a/common/op_params.py b/common/op_params.py index 70809f88f992a0..729ff2ca325b0c 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -95,7 +95,7 @@ def __init__(self): 'use_virtual_middle_line': Param(False, bool, 'For roads over 4m wide, hug right. For roads under 2m wide, hug left.'), 'uniqueID': Param(None, [type(None), str], 'User\'s unique ID'), 'autoUpdate': Param(True, bool, 'Whether to auto-update'), - 'corolla_tss2_d_tuning': Param(True, bool, 'lateral spairrowtuning using PID w/ true derivative'), + 'corolla_tss2_d_tuning': Param(False, bool, 'lateral tuning using PID w/ true derivative'), 'lat_d': Param(9.0, VT.number, 'The lateral derivative gain, default is 9.0 for TSS2 Corolla. This is active at all speeds', live=True), 'ludicrous_mode': Param(False, bool, 'Double overall acceleration!')} diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 2f86111b1217b9..3cba87ccd81982 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -153,7 +153,7 @@ class ECU: {36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8} ], CAR.RAV4_TSS2: [ - {36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1809: 8, 1872: 8, 1880:8 , 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8} + {36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1809: 8, 1841: 8, 1849: 8, 1872: 8, 1880:8 , 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8} ], CAR.COROLLA_TSS2: [ {36: 8, 37: 8, 114: 5, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 942: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1809: 8, 1816: 8, 1817: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1960: 8, 1981: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8} @@ -855,6 +855,7 @@ class ECU: b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', + b'\x02896634A18100\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152642520\x00\x00\x00\x00\x00\x00', @@ -867,6 +868,7 @@ class ECU: b'\x01F152642700\x00\x00\x00\x00\x00\x00', b'\x01F152642710\x00\x00\x00\x00\x00\x00', b'\x01F152642750\x00\x00\x00\x00\x00\x00', + ], (Ecu.eps, 0x7a1, None): [ b'8965B42170\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 860e230c7746e9..ceaa6bb1db6c9b 100755 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -38,9 +38,9 @@ def __init__(self, threadID, name): self.logger.setLevel(logging.CRITICAL) # set to logging.DEBUG to enable logging # self.logger.setLevel(logging.DEBUG) # set to logging.CRITICAL to disable logging - def save_gps_data(self, gps): + def save_gps_data(self, gps, osm_way_id): try: - location = [gps.speed, gps.bearing, gps.latitude, gps.longitude, gps.altitude, gps.accuracy, time.time()] + location = [gps.speed, gps.bearing, gps.latitude, gps.longitude, gps.altitude, gps.accuracy, time.time(), osm_way_id] with open("/data/openpilot/selfdrive/data_collection/gps-data", "a") as f: f.write("{}\n".format(location)) except: @@ -395,7 +395,7 @@ def run(self): if cur_way is not None: dat.liveMapData.wayId = cur_way.id - + self.sharedParams['osm_way_id'] = cur_way.id # Speed limit max_speed = cur_way.max_speed(heading) max_speed_ahead = None @@ -435,7 +435,8 @@ def run(self): if curvature is not None: dat.liveMapData.roadCurvatureX = [float(x) for x in dists] dat.liveMapData.roadCurvature = [float(x) for x in curvature] - + else: + self.sharedParams['osm_way_id'] = 0 if self.sharedParams['speedLimittrafficvalid']: if speedLimittraffic > 0.1: dat.liveMapData.speedLimitValid = True @@ -478,7 +479,7 @@ def run(self): self.sm.update(0) if self.sm.updated['gpsLocationExternal']: gps = self.sm['gpsLocationExternal'] - self.save_gps_data(gps) + self.save_gps_data(gps, self.sharedParams['osm_way_id']) query_lock = self.sharedParams.get('query_lock', None) @@ -577,12 +578,13 @@ def main(): speedLimittraffic = 0 speedLimittrafficvalid = False speedLimittrafficAdvisory = 0 + osm_way_id = 0 speedLimittrafficAdvisoryvalid = False sharedParams = {'last_gps' : last_gps, 'query_lock' : query_lock, 'last_query_result' : last_query_result, \ 'last_query_pos' : last_query_pos, 'cache_valid' : cache_valid, 'traffic_status' : traffic_status, \ 'traffic_confidence' : traffic_confidence, 'last_not_none_signal' : last_not_none_signal, \ 'speedLimittraffic' : speedLimittraffic, 'speedLimittrafficvalid' : speedLimittrafficvalid, \ - 'speedLimittrafficAdvisory' : speedLimittrafficAdvisory, 'speedLimittrafficAdvisoryvalid' : speedLimittrafficAdvisoryvalid} + 'speedLimittrafficAdvisory' : speedLimittrafficAdvisory, 'speedLimittrafficAdvisoryvalid' : speedLimittrafficAdvisoryvalid, 'osm_way_id' : osm_way_id} qt = QueryThread(1, "QueryThread", sharedParams=sharedParams) mt = MapsdThread(2, "MapsdThread", sharedParams=sharedParams)