mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-08-23 01:04:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot mod for 0.8.5-4

Browse Source
This commit is contained in:
Rick Lan
2021-07-02 12:49:43 +08:00
parent 145b222f0d
commit 522fb06db7
224 changed files with 9120 additions and 7777 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+40 -21
View File
@@ -32,7 +32,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
SIMULATION = "SIMULATION" in os.environ
NOSENSOR = "NOSENSOR" in os.environ
IGNORE_PROCESSES = set(["rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad"])
IGNORE_PROCESSES = set(["rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad", "dragonConf"])
ThermalStatus = log.DeviceState.ThermalStatus
State = log.ControlsState.OpenpilotState
@@ -45,6 +45,9 @@ EventName = car.CarEvent.EventName
class Controls:
def __init__(self, sm=None, pm=None, can_sock=None):
params = Params()
self.dp_jetson = params.get_bool('dp_jetson')
self.dp_panda_no_gps = params.get_bool('dp_panda_no_gps')
config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
# Setup sockets
@@ -60,9 +63,13 @@ class Controls:
self.sm = sm
if self.sm is None:
ignore = ['driverCameraState', 'managerState'] if SIMULATION else None
if self.dp_jetson:
ignore = ['driverCameraState'] if ignore is None else ignore + ['driverCameraState']
if self.dp_panda_no_gps:
ignore = ['liveLocationKalman'] if ignore is None else ignore + ['liveLocationKalman']
self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState'] + self.camera_packets,
'managerState', 'liveParameters', 'radarState', 'dragonConf'] + self.camera_packets,
ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
self.can_sock = can_sock
@@ -80,7 +87,6 @@ class Controls:
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'])
# read params
params = Params()
self.is_metric = params.get_bool("IsMetric")
self.is_ldw_enabled = params.get_bool("IsLdwEnabled")
self.enable_lte_onroad = params.get_bool("EnableLteOnroad")
@@ -115,7 +121,9 @@ class Controls:
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
if params.get_bool('dp_lqr'):
self.LaC = LatControlLQR(self.CP)
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
@@ -148,8 +156,8 @@ class Controls:
self.startup_event = get_startup_event(car_recognized, controller_available, self.CP.fuzzyFingerprint,
len(self.CP.carFw) > 0)
if not sounds_available:
self.events.add(EventName.soundsUnavailable, static=True)
# if not sounds_available:
# self.events.add(EventName.soundsUnavailable, static=True)
if community_feature_disallowed and car_recognized:
self.events.add(EventName.communityFeatureDisallowed, static=True)
if not car_recognized:
@@ -161,6 +169,11 @@ class Controls:
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
# dp
self.sm['dragonConf'].dpAtl = False
self.sm['dragonConf'].dpSrCustom = self.CP.steerRatio
self.sm['dragonConf'].dpSrLearner = True
def update_events(self, CS):
"""Compute carEvents from carState"""
@@ -179,9 +192,9 @@ class Controls:
return
# Create events for battery, temperature, disk space, and memory
if self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
# at zero percent battery, while discharging, OP should not allowed
self.events.add(EventName.lowBattery)
# if self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
# # at zero percent battery, while discharging, OP should not allowed
# self.events.add(EventName.lowBattery)
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7:
@@ -215,15 +228,15 @@ class Controls:
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
self.events.add(EventName.preLaneChangeLeft)
self.events.add(EventName.preLaneChangeLeftALC if self.sm['lateralPlan'].dpALCAllowed else EventName.preLaneChangeLeft)
else:
self.events.add(EventName.preLaneChangeRight)
self.events.add(EventName.preLaneChangeRightALC if self.sm['lateralPlan'].dpALCAllowed else EventName.preLaneChangeRight)
elif self.sm['lateralPlan'].laneChangeState in [LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing]:
self.events.add(EventName.laneChange)
if self.can_rcv_error or not CS.canValid:
self.events.add(EventName.canError)
self.events.add(EventName.pcmDisable if self.sm['dragonConf'].dpAtl else EventName.canError)
safety_mismatch = self.sm['pandaState'].safetyModel != self.CP.safetyModel or self.sm['pandaState'].safetyParam != self.CP.safetyParam
if safety_mismatch or self.mismatch_counter >= 200:
@@ -236,7 +249,7 @@ class Controls:
self.events.add(EventName.radarFault)
elif not self.sm.valid["pandaState"]:
self.events.add(EventName.usbError)
elif not self.sm.all_alive_and_valid():
elif not self.dp_jetson and not self.sm.all_alive_and_valid():
self.events.add(EventName.commIssue)
if not self.logged_comm_issue:
cloudlog.error(f"commIssue - valid: {self.sm.valid} - alive: {self.sm.alive}")
@@ -245,8 +258,8 @@ class Controls:
self.logged_comm_issue = False
if not self.sm['lateralPlan'].mpcSolutionValid:
self.events.add(EventName.plannerError)
if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
self.events.add(EventName.steerTempUnavailableUserOverride if self.sm['dragonConf'].dpAtl else EventName.plannerError)
if not self.dp_panda_no_gps and not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
self.events.add(EventName.sensorDataInvalid)
if not self.sm['liveLocationKalman'].posenetOK:
@@ -280,16 +293,16 @@ class Controls:
# TODO: fix simulator
if not SIMULATION:
if not NOSENSOR:
if not self.dp_panda_no_gps and not NOSENSOR:
if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and \
(not TICI or self.enable_lte_onroad):
# Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
self.events.add(EventName.noGps)
if not self.sm.all_alive(self.camera_packets):
if not self.dp_jetson and not self.sm.all_alive(self.camera_packets):
self.events.add(EventName.cameraMalfunction)
if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging)
if self.sm['liveLocationKalman'].excessiveResets:
if not self.dp_panda_no_gps and self.sm['liveLocationKalman'].excessiveResets:
self.events.add(EventName.localizerMalfunction)
# Check if all manager processes are running
@@ -298,7 +311,7 @@ class Controls:
self.events.add(EventName.processNotRunning)
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and self.sm['longitudinalPlan'].vTargetFuture >= STARTING_TARGET_SPEED \
if not self.sm['dragonConf'].dpAtl and CS.brakePressed and self.sm['longitudinalPlan'].vTargetFuture >= STARTING_TARGET_SPEED \
and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
self.events.add(EventName.noTarget)
@@ -307,7 +320,7 @@ class Controls:
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs)
CS = self.CI.update(self.CC, can_strs, self.sm['dragonConf'])
self.sm.update(0)
@@ -330,7 +343,7 @@ class Controls:
if not self.enabled:
self.mismatch_counter = 0
if not self.sm['pandaState'].controlsAllowed and self.enabled:
if not self.sm['dragonConf'].dpAtl and not self.sm['pandaState'].controlsAllowed and self.enabled:
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
@@ -421,6 +434,11 @@ class Controls:
params = self.sm['liveParameters']
x = max(params.stiffnessFactor, 0.1)
sr = max(params.steerRatio, 0.1)
if not self.sm['dragonConf'].dpSrLearner:
if self.sm['dragonConf'].dpSrCustom >= 10:
sr = self.sm['dragonConf'].dpSrCustom
else:
sr = self.CP.steerRatio
self.VM.update_params(x, sr)
lat_plan = self.sm['lateralPlan']
@@ -557,6 +575,7 @@ class Controls:
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.curvature = curvature
controlsState.angleSteers = CS.steeringAngleDeg
controlsState.steeringAngleDesiredDeg = angle_steers_des
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)