mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-07-07 13:52:06 +08:00
Merge branch 'devel-zht' into 0.6.4-zht
This commit is contained in:
dragonpilot
Binary file not shown.
@@ -104,6 +104,7 @@ class CarInterface(object):
|
||||
self.dragon_enable_steering_on_signal = False
|
||||
self.dragon_allow_gas = False
|
||||
self.ts_last_check = 0.
|
||||
self.dragon_lat_ctrl = True
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
@@ -387,6 +388,7 @@ class CarInterface(object):
|
||||
if ts - self.ts_last_check > 5.:
|
||||
self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
|
||||
self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
|
||||
self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True
|
||||
self.ts_last_check = ts
|
||||
|
||||
# ******************* do can recv *******************
|
||||
@@ -503,7 +505,7 @@ class CarInterface(object):
|
||||
# wait 1.0s before throwing the alert to avoid it popping when you turn off the car
|
||||
if self.cp_cam.can_invalid_cnt >= 100 and self.CS.CP.carFingerprint not in HONDA_BOSCH and self.CP.enableCamera:
|
||||
events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
if not self.CS.lkMode:
|
||||
if not self.CS.lkMode or not self.dragon_lat_ctrl:
|
||||
events.append(create_event('manualSteeringRequired', [ET.WARNING]))
|
||||
elif self.CS.lkMode and (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal:
|
||||
events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING]))
|
||||
|
||||
@@ -79,8 +79,8 @@ FINGERPRINTS = {
|
||||
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 450: 8, 464: 8, 470: 2, 476: 7, 487: 4, 490: 8, 493: 5, 506: 8, 512: 6, 513: 6, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1633: 8,
|
||||
}],
|
||||
CAR.CIVIC_BOSCH: [{
|
||||
# 2017 Civic Hatchback EX and 2019 Civic Sedan Touring Canadian
|
||||
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1633: 8,
|
||||
# 2017 Civic Hatchback EX, 2019 Civic Sedan Touring Canadian, and 2018 Civic Hatchback Executive Premium 1.0L CVT European
|
||||
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 460: 3, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1625: 5, 1629: 5, 1633: 8,
|
||||
}],
|
||||
CAR.CRV: [{
|
||||
57: 3, 145: 8, 316: 8, 340: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 401: 8, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 506: 8, 507: 1, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8,
|
||||
|
||||
@@ -41,6 +41,7 @@ class CarInterface(object):
|
||||
self.dragon_enable_steering_on_signal = False
|
||||
self.dragon_allow_gas = False
|
||||
self.ts_last_check = 0.
|
||||
self.dragon_lat_ctrl = True
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
@@ -315,7 +316,8 @@ class CarInterface(object):
|
||||
if ts - self.ts_last_check > 5.:
|
||||
self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True
|
||||
self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True
|
||||
self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
|
||||
self.dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU") == "1" else False
|
||||
self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True
|
||||
self.ts_last_check = ts
|
||||
|
||||
# ******************* do can recv *******************
|
||||
@@ -420,7 +422,9 @@ class CarInterface(object):
|
||||
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
|
||||
if ret.gearShifter == GearShifter.reverse and self.CP.enableDsu:
|
||||
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
if (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal:
|
||||
if not self.dragon_lat_ctrl:
|
||||
events.append(create_event('manualSteeringRequired', [ET.WARNING]))
|
||||
elif (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal:
|
||||
events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING]))
|
||||
elif self.CS.steer_error:
|
||||
events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
|
||||
|
||||
@@ -183,11 +183,15 @@ FINGERPRINTS = {
|
||||
# XLE, Limited, and AWD
|
||||
{
|
||||
36: 8, 37: 8, 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, 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, 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, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
|
||||
},
|
||||
# Taiwan 2019 RAV4H from Max Duan
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 913: 8, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 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, 1594: 8, 1595: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8
|
||||
}],
|
||||
CAR.COROLLA_TSS2: [
|
||||
# hatch 2019+ and sedan 2020+
|
||||
{
|
||||
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, 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, 921: 8, 933: 8, 934: 8, 935: 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
|
||||
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, 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, 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
|
||||
},
|
||||
{
|
||||
# 2019 Taiwan Altis
|
||||
|
||||
@@ -1,64 +0,0 @@
|
||||
from numpy import clip
|
||||
import pickle
|
||||
import csv
|
||||
import os
|
||||
|
||||
# HOW TO
|
||||
# import this module to where you want to use it, such as from ```selfdrive.controls.lib.curvature_learner import CurvatureLearner```
|
||||
# create the object ```self.curvature_offset = CurvatureLearner(debug=False)```
|
||||
# call the update method ```self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly)```
|
||||
# The learned curvature offsets will save and load automatically
|
||||
# If you still need help, check out how I have it implemented in the devel_curvaturefactorlearner branch
|
||||
# by Zorrobyte
|
||||
# version 4
|
||||
|
||||
class CurvatureLearner:
|
||||
def __init__(self, debug=False):
|
||||
self.offset = 0.
|
||||
self.learning_rate = 12000
|
||||
self.frame = 0
|
||||
self.debug = debug
|
||||
try:
|
||||
self.learned_offsets = pickle.load(open("/data/curvaturev4.p", "rb"))
|
||||
except (OSError, IOError):
|
||||
self.learned_offsets = {
|
||||
"center": 0.,
|
||||
"inner": 0.,
|
||||
"outer": 0.
|
||||
}
|
||||
pickle.dump(self.learned_offsets, open("/data/curvaturev4.p", "wb"))
|
||||
os.chmod("/data/curvaturev4.p", 0o777)
|
||||
|
||||
def update(self, angle_steers=0., d_poly=None, v_ego=0.):
|
||||
if angle_steers > 0.1:
|
||||
if abs(angle_steers) < 2.:
|
||||
self.learned_offsets["center"] -= d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["center"]
|
||||
elif 2. < abs(angle_steers) < 5.:
|
||||
self.learned_offsets["inner"] -= d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["inner"]
|
||||
elif abs(angle_steers) > 5.:
|
||||
self.learned_offsets["outer"] -= d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["outer"]
|
||||
elif angle_steers < -0.1:
|
||||
if abs(angle_steers) < 2.:
|
||||
self.learned_offsets["center"] += d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["center"]
|
||||
elif 2. < abs(angle_steers) < 5.:
|
||||
self.learned_offsets["inner"] += d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["inner"]
|
||||
elif abs(angle_steers) > 5.:
|
||||
self.learned_offsets["outer"] += d_poly[3] / self.learning_rate
|
||||
self.offset = self.learned_offsets["outer"]
|
||||
|
||||
self.offset = clip(self.offset, -0.3, 0.3)
|
||||
self.frame += 1
|
||||
|
||||
if self.frame == 12000: # every 2 mins
|
||||
pickle.dump(self.learned_offsets, open("/data/curvaturev4.p", "wb"))
|
||||
self.frame = 0
|
||||
if self.debug:
|
||||
with open('/data/curvdebug.csv', 'a') as csv_file:
|
||||
csv_file_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
|
||||
csv_file_writer.writerow([self.learned_offsets, v_ego])
|
||||
return self.offset
|
||||
@@ -9,7 +9,6 @@ from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
|
||||
from selfdrive.controls.lib.lane_planner import LanePlanner
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.controls.lib.curvature_learner import CurvatureLearner
|
||||
|
||||
|
||||
LOG_MPC = os.environ.get('LOG_MPC', False)
|
||||
@@ -30,7 +29,6 @@ class PathPlanner(object):
|
||||
self.setup_mpc(CP.steerRateCost)
|
||||
self.solution_invalid_cnt = 0
|
||||
self.path_offset_i = 0.0
|
||||
self.curvature_offset = CurvatureLearner(debug=False)
|
||||
|
||||
def setup_mpc(self, steer_rate_cost):
|
||||
self.libmpc = libmpc_py.libmpc
|
||||
@@ -60,13 +58,7 @@ class PathPlanner(object):
|
||||
# Run MPC
|
||||
self.angle_steers_des_prev = self.angle_steers_des_mpc
|
||||
VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
|
||||
#curvature_factor = VM.curvature_factor(v_ego)
|
||||
if active:
|
||||
curvfac = self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly, v_ego)
|
||||
else:
|
||||
curvfac = 0.
|
||||
|
||||
curvature_factor = VM.curvature_factor(v_ego) + curvfac
|
||||
curvature_factor = VM.curvature_factor(v_ego)
|
||||
|
||||
# TODO: Check for active, override, and saturation
|
||||
# if active:
|
||||
|
||||
@@ -172,6 +172,7 @@ def thermald_thread():
|
||||
# clear car params when panda gets disconnected
|
||||
if health is None and health_prev is not None:
|
||||
params.panda_disconnect()
|
||||
ignition_seen = False
|
||||
health_prev = health
|
||||
|
||||
# loggerd is gated based on free space
|
||||
|
||||
Reference in New Issue
Block a user