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

dragonpilot beta3

Browse Source 
date: 2023-08-22T14:21:17
commit: 6148ce3d77530281f890970718e9c42b2acc5ff1
This commit is contained in:
dragonpilot
2023-08-22 14:20:25 -07:00
parent 957d61c2f7
commit d8e5331c6e
722 changed files with 24317 additions and 116310 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+17 -21
View File
@@ -43,8 +43,6 @@ NOSENSOR = "NOSENSOR" in os.environ
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
# dp related processes
IGNORE_PROCESSES.update({"mapd", "otisserv", "fileserv", "gpxd", "gpx_uploader"})
if dp_device_dm_unavailable:
IGNORE_PROCESSES.update({"dmonitoringd", "dmonitoringmodeld"})
ThermalStatus = log.DeviceState.ThermalStatus
State = log.ControlsState.OpenpilotState
@@ -103,8 +101,6 @@ class Controls:
ignore = ['testJoystick']
if SIMULATION:
ignore += ['driverCameraState', 'managerState']
if dp_device_dm_unavailable:
ignore += ['driverMonitoringState']
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick', 'longitudinalPlanExt'] + self.camera_packets,
@@ -304,7 +300,7 @@ class Controls:
if CS.gasPressed:
self.events.add(EventName.gasPressedOverride)
if not dp_device_dm_unavailable and not self.CP.notCar:
if not self.CP.notCar:
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
# Add car events, ignore if CAN isn't valid
@@ -410,7 +406,7 @@ class Controls:
# generic catch-all. ideally, a more specific event should be added above instead
can_rcv_timeout = self.can_rcv_timeout_counter >= 5
has_disable_events = self.events.any(ET.NO_ENTRY) and (self.events.any(ET.SOFT_DISABLE) or self.events.any(ET.IMMEDIATE_DISABLE))
has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
if (not self.sm.all_checks() or can_rcv_timeout) and no_system_errors:
if not self.sm.all_alive():
@@ -546,29 +542,29 @@ class Controls:
# ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING
if self.state != State.disabled:
# user and immediate disable always have priority in a non-disabled state
if self.events.any(ET.USER_DISABLE):
if self.events.contains(ET.USER_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.USER_DISABLE)
elif self.events.any(ET.IMMEDIATE_DISABLE):
elif self.events.contains(ET.IMMEDIATE_DISABLE):
self.state = State.disabled
self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
else:
# ENABLED
if self.state == State.enabled:
if self.events.any(ET.SOFT_DISABLE):
if self.events.contains(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
elif self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL):
elif self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL):
self.state = State.overriding
self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
# SOFT DISABLING
elif self.state == State.softDisabling:
if not self.events.any(ET.SOFT_DISABLE):
if not self.events.contains(ET.SOFT_DISABLE):
# no more soft disabling condition, so go back to ENABLED
self.state = State.enabled
@@ -580,32 +576,32 @@ class Controls:
# PRE ENABLING
elif self.state == State.preEnabled:
if not self.events.any(ET.PRE_ENABLE):
if not self.events.contains(ET.PRE_ENABLE):
self.state = State.enabled
else:
self.current_alert_types.append(ET.PRE_ENABLE)
# OVERRIDING
elif self.state == State.overriding:
if self.events.any(ET.SOFT_DISABLE):
if self.events.contains(ET.SOFT_DISABLE):
self.state = State.softDisabling
self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.current_alert_types.append(ET.SOFT_DISABLE)
elif not (self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL)):
elif not (self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL)):
self.state = State.enabled
else:
self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
# DISABLED
elif self.state == State.disabled:
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
if self.events.contains(ET.ENABLE):
if self.events.contains(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
else:
if self.events.any(ET.PRE_ENABLE):
if self.events.contains(ET.PRE_ENABLE):
self.state = State.preEnabled
elif self.events.any(ET.OVERRIDE_LATERAL) or self.events.any(ET.OVERRIDE_LONGITUDINAL):
elif self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL):
self.state = State.overriding
else:
self.state = State.enabled
@@ -644,7 +640,7 @@ class Controls:
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.joystick_mode)
CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
if not self.read_only and self.initialized and self._dp_alka and self._dp_alka_active and not standstill and CS.cruiseState.available:
if self.sm['liveCalibration'].calStatus != log.LiveCalibrationData.Status.calibrated:
@@ -822,7 +818,7 @@ class Controls:
else:
self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2
force_decel = (not dp_device_dm_unavailable and self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
# Curvature & Steering angle
@@ -852,7 +848,7 @@ class Controls:
controlsState.desiredCurvature = self.desired_curvature
controlsState.desiredCurvatureRate = self.desired_curvature_rate
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.engageable = not self.events.contains(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph)