sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Controlsd refactor (#1487)

Browse Source 
* it's a class

* more refactor

* remove that

* car interface should create that

* that too

* not a dict

* don't create permanent events every iteration

* break up long lines

* fix honda

* small optimization

* less long lines

* dict is faster

* latcontrol less args

* longcontrol less args

* update profiling script

* few optimizations

* create events together

* clean up

* more clean up

* remove comment

* clean up

* simplify state transition

* more clean up

* update comments
old-commit-hash: 08832ff29d
commatwo_master
Adeeb 5 years ago committed by GitHub
parent e8ae840fa7
commit fe106e25a3
7 changed files with 534 additions and 534 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      selfdrive/car/interfaces.py
  2. 811
      selfdrive/controls/controlsd.py
  3. 10
      selfdrive/controls/lib/latcontrol_indi.py
  4. 20
      selfdrive/controls/lib/latcontrol_lqr.py
  5. 18
      selfdrive/controls/lib/latcontrol_pid.py
  6. 20
      selfdrive/controls/lib/longcontrol.py
  7. 2
      selfdrive/test/profiling/controlsd.py

5
selfdrive/car/interfaces.py
Unescape View File

@ -4,6 +4,7 @@ from cereal import car
from common.kalman.simple_kalman import KF1D from common.kalman.simple_kalman import KF1D
from common.realtime import DT_CTRL from common.realtime import DT_CTRL
from selfdrive.car import gen_empty_fingerprint from selfdrive.car import gen_empty_fingerprint
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -96,6 +97,10 @@ class CarInterfaceBase():
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if cs_out.gasPressed: if cs_out.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if cs_out.stockAeb:
events.append(create_event('stockAeb', []))
if cs_out.vEgo > 92 * CV.MPH_TO_MS:
events.append(create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if cs_out.steerError: if cs_out.steerError:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))

811
selfdrive/controls/controlsd.py
Unescape View File

@ -26,6 +26,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.controls.lib.planner import LON_MPC_STEP
from selfdrive.locationd.calibration_helpers import Calibration, Filter from selfdrive.locationd.calibration_helpers import Calibration, Filter
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1 LANE_DEPARTURE_THRESHOLD = 0.1
STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL
STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
@ -33,30 +34,12 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
ThermalStatus = log.ThermalData.ThermalStatus ThermalStatus = log.ThermalData.ThermalStatus
State = log.ControlsState.OpenpilotState State = log.ControlsState.OpenpilotState
HwType = log.HealthData.HwType HwType = log.HealthData.HwType
LongitudinalPlanSource = log.Plan.LongitudinalPlanSource
Desire = log.PathPlan.Desire
LaneChangeState = log.PathPlan.LaneChangeState LaneChangeState = log.PathPlan.LaneChangeState
LaneChangeDirection = log.PathPlan.LaneChangeDirection LaneChangeDirection = log.PathPlan.LaneChangeDirection
def add_lane_change_event(events, path_plan):
if path_plan.laneChangeState == LaneChangeState.preLaneChange:
if path_plan.laneChangeDirection == LaneChangeDirection.left:
events.append(create_event('preLaneChangeLeft', [ET.WARNING]))
else:
events.append(create_event('preLaneChangeRight', [ET.WARNING]))
elif path_plan.laneChangeState in [LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing]:
events.append(create_event('laneChange', [ET.WARNING]))
def isActive(state):
"""Check if the actuators are enabled"""
return state in [State.enabled, State.softDisabling]
def isEnabled(state):
"""Check if openpilot is engaged"""
return (isActive(state) or state == State.preEnabled)
def events_to_bytes(events): def events_to_bytes(events):
# optimization when comparing capnp structs: str() or tree traverse are much slower # optimization when comparing capnp structs: str() or tree traverse are much slower
ret = [] ret = []
@ -69,540 +52,550 @@ def events_to_bytes(events):
return ret return ret
def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params): class Controls:
"""Receive data from sockets and create events for battery, temperature and disk space""" def __init__(self, sm=None, pm=None, can_sock=None):
gc.disable()
set_realtime_priority(3)
# Setup sockets
self.pm = pm
if self.pm is None:
self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', \
'carControl', 'carEvents', 'carParams'])
# Update carstate from CAN and create events self.sm = sm
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) if self.sm is None:
CS = CI.update(CC, can_strs) self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', \
'dMonitoringState', 'plan', 'pathPlan'])
sm.update(0) self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
events = list(CS.events) # wait for one health and one CAN packet
events += list(sm['dMonitoringState'].events) hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
add_lane_change_event(events, sm['pathPlan']) has_relay = hw_type in [HwType.blackPanda, HwType.uno]
enabled = isEnabled(state) print("Waiting for CAN messages...")
messaging.get_one_can(self.can_sock)
# Check for CAN timeout self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay)
if not can_strs:
can_error_counter += 1
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red # read params
free_space = sm['thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed params = Params()
low_battery = sm['thermal'].batteryPercent < 1 and sm['thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed self.is_metric = params.get("IsMetric", encoding='utf8') == "1"
mem_low = sm['thermal'].memUsedPercent > 90 self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None
community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1" or \
internet_needed or not openpilot_enabled_toggle
car_recognized = self.CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not dashcam mode
controller_available = self.CP.enableCamera and self.CI.CC is not None and not passive
community_feature_disallowed = self.CP.communityFeature and not community_feature_toggle
self.read_only = not car_recognized or not controller_available or \
self.CP.dashcamOnly or community_feature_disallowed
if self.read_only:
self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Create events for battery, temperature and disk space # Write CarParams for radard and boardd safety mode
if low_battery: cp_bytes = self.CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("LongitudinalControl", "1" if self.CP.openpilotLongitudinalControl else "0")
self.CC = car.CarControl.new_message()
self.AM = AlertManager()
self.LoC = LongControl(self.CP, self.CI.compute_gb)
self.VM = VehicleModel(self.CP)
if self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP)
self.state = State.disabled
self.enabled = False
self.active = False
self.can_rcv_error = False
self.soft_disable_timer = 0
self.v_cruise_kph = 255
self.v_cruise_kph_last = 0
self.mismatch_counter = 0
self.can_error_counter = 0
self.last_blinker_frame = 0
self.saturated_count = 0
self.events_prev = ""
self.sm['liveCalibration'].calStatus = Calibration.INVALID
self.sm['pathPlan'].sensorValid = True
self.sm['pathPlan'].posenetValid = True
self.sm['thermal'].freeSpace = 1.
self.sm['dMonitoringState'].events = []
self.sm['dMonitoringState'].awarenessStatus = 1.
self.sm['dMonitoringState'].faceDetected = False
startup_alert = get_startup_alert(car_recognized, controller_available)
self.AM.add(self.sm.frame, startup_alert, False)
# controlsd is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
self.prof = Profiler(False) # off by default
# detect sound card presence and ensure successful init
sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') \
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
self.static_events = []
if not sounds_available:
self.static_events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
if internet_needed:
self.static_events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
if community_feature_disallowed:
self.static_events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT]))
if self.read_only and not passive:
self.static_events.append(create_event('carUnrecognized', [ET.PERMANENT]))
def create_events(self, CS):
"""Compute carEvents from carState"""
events = self.static_events.copy()
events.extend(CS.events)
events.extend(self.sm['dMonitoringState'].events)
# Create events for battery, temperature, disk space, and memory
if self.sm['thermal'].batteryPercent < 1 and self.sm['thermal'].chargingError:
# at zero percent battery, while discharging, OP should not allowed
events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if overtemp: if self.sm['thermal'].thermalStatus >= ThermalStatus.red:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space: if self.sm['thermal'].freeSpace < 0.07:
# under 7% of space free no enable allowed
events.append(create_event('outOfSpace', [ET.NO_ENTRY])) events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
if mem_low: if self.sm['thermal'].memUsedPercent > 90:
events.append(create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) events.append(create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
if CS.stockAeb: # Handle calibration status
events.append(create_event('stockAeb', [])) cal_status = self.sm['liveCalibration'].calStatus
# Handle calibration
cal_status = sm['liveCalibration'].calStatus
cal_perc = sm['liveCalibration'].calPerc
if cal_status != Calibration.CALIBRATED: if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED: if cal_status == Calibration.UNCALIBRATED:
events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else: else:
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if CS.vEgo > 92 * CV.MPH_TO_MS: # Handle lane change
events.append(create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.sm['pathPlan'].laneChangeState == LaneChangeState.preLaneChange:
if self.sm['path_plan'].laneChangeDirection == LaneChangeDirection.left:
events.append(create_event('preLaneChangeLeft', [ET.WARNING]))
else:
events.append(create_event('preLaneChangeRight', [ET.WARNING]))
elif self.sm['pathPlan'].laneChangeState in [LaneChangeState.laneChangeStarting, \
LaneChangeState.laneChangeFinishing]:
events.append(create_event('laneChange', [ET.WARNING]))
if self.can_rcv_error:
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.mismatch_counter >= 200:
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
if not self.sm.alive['plan'] and self.sm.alive['pathPlan']:
# only plan not being received: radar not communicating
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif not self.sm.all_alive_and_valid():
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.sm['pathPlan'].mpcSolutionValid:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not self.sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None:
events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not self.sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not self.sm['pathPlan'].posenetValid:
events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
if not self.sm['plan'].radarValid:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.sm['plan'].radarCanError:
events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if log.HealthData.FaultType.relayMalfunction in self.sm['health'].faults:
events.append(create_event('relayMalfunction', [ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \
and not self.CP.radarOffCan and CS.vEgo < 0.3:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# TODO: clean up this alert creation in alerts refactor
if self.active:
for e in get_events(events, [ET.WARNING]):
# TODO: handle non static text in a cleaner way, like a callback
extra_text = ""
if e == "belowSteerSpeed":
if self.is_metric:
extra_text = str(int(round(self.CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
else:
extra_text = str(int(round(self.CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
self.AM.add(self.sm.frame, e, self.enabled, extra_text_2=extra_text)
for e in get_events(events, [ET.PERMANENT]):
# TODO: handle non static text in a cleaner way, like a callback
extra_text_1, extra_text_2 = "", ""
if e == "calibrationIncomplete":
extra_text_1 = str(self.sm['liveCalibration'].calPerc) + "%"
if self.is_metric:
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
else:
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
self.AM.add(self.sm.frame, str(e) + "Permanent", self.enabled, \
extra_text_1=extra_text_1, extra_text_2=extra_text_2)
return events
def data_sample(self):
"""Receive data from sockets and update carState"""
# 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)
self.sm.update(0)
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed # When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through # we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other. # another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement. # Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not enabled: if not self.enabled:
mismatch_counter = 0 self.mismatch_counter = 0
controls_allowed = sm['health'].controlsAllowed if not self.sm['health'].controlsAllowed and self.enabled:
if not controls_allowed and enabled: self.mismatch_counter += 1
mismatch_counter += 1
if mismatch_counter >= 200:
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
return CS, events, cal_perc, mismatch_counter, can_error_counter return CS
def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): def state_transition(self, CS, events):
"""Compute conditional state transitions and execute actions on state transitions""" """Compute conditional state transitions and execute actions on state transitions"""
enabled = isEnabled(state)
v_cruise_kph_last = v_cruise_kph self.v_cruise_kph_last = self.v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic # if stock cruise is completely disabled, then we can use our own set speed logic
if not CP.enableCruise: if not self.CP.enableCruise:
v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled) self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.enabled)
elif CP.enableCruise and CS.cruiseState.enabled: elif self.CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on # decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state # entrance in SOFT_DISABLING state
soft_disable_timer = max(0, soft_disable_timer - 1) self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
# DISABLED alert_types = []
if state == State.disabled:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
AM.add(frame, str(e) + "NoEntry", enabled)
else: # ENABLED, PRE ENABLING, SOFT DISABLING
if get_events(events, [ET.PRE_ENABLE]): if self.state != State.disabled:
state = State.preEnabled # user and immediate disable always have priority in a non-disabled state
else:
state = State.enabled
AM.add(frame, "enable", enabled)
v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, v_cruise_kph_last)
# ENABLED
elif state == State.enabled:
if get_events(events, [ET.USER_DISABLE]): if get_events(events, [ET.USER_DISABLE]):
state = State.disabled self.state = State.disabled
AM.add(frame, "disable", enabled) self.AM.add(self.sm.frame, "disable", self.enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]): elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled self.state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]): alert_types = [ET.IMMEDIATE_DISABLE]
AM.add(frame, e, enabled)
elif get_events(events, [ET.SOFT_DISABLE]): else:
state = State.softDisabling # ENABLED
soft_disable_timer = 300 # 3s if self.state == State.enabled:
for e in get_events(events, [ET.SOFT_DISABLE]): if get_events(events, [ET.SOFT_DISABLE]):
AM.add(frame, e, enabled) self.state = State.softDisabling
self.soft_disable_timer = 300 # 3s
alert_types = [ET.SOFT_DISABLE]
# SOFT DISABLING # SOFT DISABLING
elif state == State.softDisabling: elif self.state == State.softDisabling:
if get_events(events, [ET.USER_DISABLE]): if not get_events(events, [ET.SOFT_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(frame, e, enabled)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED # no more soft disabling condition, so go back to ENABLED
state = State.enabled self.state = State.enabled
elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0: elif get_events(events, [ET.SOFT_DISABLE]) and self.soft_disable_timer > 0:
for e in get_events(events, [ET.SOFT_DISABLE]): alert_types = [ET.SOFT_DISABLE]
AM.add(frame, e, enabled)
elif soft_disable_timer <= 0: elif self.soft_disable_timer <= 0:
state = State.disabled self.state = State.disabled
# PRE ENABLING # PRE ENABLING
elif state == State.preEnabled: elif self.state == State.preEnabled:
if get_events(events, [ET.USER_DISABLE]): if not get_events(events, [ET.PRE_ENABLE]):
state = State.disabled self.state = State.enabled
AM.add(frame, "disable", enabled)
# DISABLED
elif self.state == State.disabled:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
self.AM.add(self.sm.frame, str(e) + "NoEntry", self.enabled)
else:
if get_events(events, [ET.PRE_ENABLE]):
self.state = State.preEnabled
else:
self.state = State.enabled
self.AM.add(self.sm.frame, "enable", self.enabled)
self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): for e in get_events(events, alert_types):
state = State.disabled self.AM.add(self.sm.frame, e, self.enabled)
for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
elif not get_events(events, [ET.PRE_ENABLE]): # Check if actuators are enabled
state = State.enabled self.active = self.state == State.enabled or self.state == State.softDisabling
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last # Check if openpilot is engaged
self.enabled = self.active or self.state == State.preEnabled
def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, def state_control(self, CS, events):
AM, rk, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count):
"""Given the state, this function returns an actuators packet""" """Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message() plan = self.sm['plan']
path_plan = self.sm['pathPlan']
enabled = isEnabled(state) actuators = car.CarControl.Actuators.new_message()
active = isActive(state)
if CS.leftBlinker or CS.rightBlinker: if CS.leftBlinker or CS.rightBlinker:
last_blinker_frame = frame self.last_blinker_frame = self.sm.frame
if plan.fcw: if plan.fcw:
# send FCW alert if triggered by planner # send FCW alert if triggered by planner
AM.add(frame, "fcw", enabled) self.AM.add(self.sm.frame, "fcw", self.enabled)
elif CS.stockFcw: elif CS.stockFcw:
# send a silent alert when stock fcw triggers, since the car is already beeping # send a silent alert when stock fcw triggers, since the car is already beeping
AM.add(frame, "fcwStock", enabled) self.AM.add(self.sm.frame, "fcwStock", self.enabled)
# State specific actions # State specific actions
if state in [State.preEnabled, State.disabled]: if not self.active:
LaC.reset() self.LaC.reset()
LoC.reset(v_pid=CS.vEgo) self.LoC.reset(v_pid=CS.vEgo)
elif state in [State.enabled, State.softDisabling]: plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['plan'])
# parse warnings from car specific interface # no greater than dt mpc + dt, to prevent too high extraps
for e in get_events(events, [ET.WARNING]): dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL
extra_text = ""
if e == "belowSteerSpeed":
if is_metric:
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
else:
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
AM.add(frame, e, enabled, extra_text_2=extra_text)
plan_age = DT_CTRL * (frame - rcv_frame['plan'])
dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps
a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart) a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart)
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
# Gas/Brake PID loop # Gas/Brake PID loop
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP)
v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
# Steering PID loop and lateral MPC # Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan) actuators.steer, actuators.steerAngle, lac_log = self.LaC.update(self.active, CS, self.CP, path_plan)
# Check for difference between desired angle and angle for angle based control # Check for difference between desired angle and angle for angle based control
angle_control_saturated = CP.steerControlType == car.CarParams.SteerControlType.angle and \ angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD
saturated_count = saturated_count + 1 if angle_control_saturated and not CS.steeringPressed and active else 0 if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
# Send a "steering required alert" if saturation count has reached the limit # Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or (saturated_count > STEER_ANGLE_SATURATION_TIMEOUT): if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
# Check if we deviated from the path # Check if we deviated from the path
left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1 left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1 right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
if left_deviation or right_deviation: if left_deviation or right_deviation:
AM.add(frame, "steerSaturated", enabled) self.AM.add(self.sm.frame, "steerSaturated", self.enabled)
# Parse permanent warnings to display constantly
for e in get_events(events, [ET.PERMANENT]):
extra_text_1, extra_text_2 = "", ""
if e == "calibrationIncomplete":
extra_text_1 = str(cal_perc) + "%"
if is_metric:
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
else:
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
return actuators, v_cruise_kph, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame, saturated_count return actuators, v_acc_sol, a_acc_sol, lac_log
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, def publish_logs(self, CS, events, start_time, actuators, v_acc, a_acc, lac_log):
LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev,
last_blinker_frame, is_ldw_enabled, can_error_counter):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging""" """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message() CC = car.CarControl.new_message()
CC.enabled = isEnabled(state) CC.enabled = self.enabled
CC.actuators = actuators CC.actuators = actuators
CC.cruiseControl.override = True CC.cruiseControl.override = True
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled) CC.cruiseControl.cancel = not self.CP.enableCruise or (not self.enabled and CS.cruiseState.enabled)
# Some override values for Honda # Some override values for Honda
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) # brake discount removes a sharp nonlinearity # brake discount removes a sharp nonlinearity
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0) brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget) speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount)
CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0)
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['plan'].aTarget, CS.vEgo, self.sm['plan'].vTarget)
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.leadVisible = sm['plan'].hasLead CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
right_lane_visible = sm['pathPlan'].rProb > 0.5 CC.hudControl.leadVisible = self.sm['plan'].hasLead
left_lane_visible = sm['pathPlan'].lProb > 0.5
right_lane_visible = self.sm['pathPlan'].rProb > 0.5
left_lane_visible = self.sm['pathPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible)
recent_blinker = (sm.frame - last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
calibrated = sm['liveCalibration'].calStatus == Calibration.CALIBRATED ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
ldw_allowed = CS.vEgo > 31 * CV.MPH_TO_MS and not recent_blinker and is_ldw_enabled and not isActive(state) and calibrated and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
md = sm['model']
if len(md.meta.desirePrediction):
l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1]
r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1]
l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) meta = self.sm['model'].meta
r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) if len(meta.desirePrediction) and ldw_allowed:
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET))
r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET))
if ldw_allowed:
CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
AM.add(sm.frame, 'ldwPermanent', False) self.AM.add(self.sm.frame, 'ldwPermanent', False)
events.append(create_event('ldw', [ET.PERMANENT])) events.append(create_event('ldw', [ET.PERMANENT]))
AM.process_alerts(sm.frame) self.AM.process_alerts(self.sm.frame)
CC.hudControl.visualAlert = AM.visual_alert CC.hudControl.visualAlert = self.AM.visual_alert
if not read_only: if not self.read_only:
# send car controls over can # send car controls over can
can_sends = CI.apply(CC) can_sends = self.CI.apply(CC)
pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
force_decel = (sm['dMonitoringState'].awarenessStatus < 0.) or (state == State.softDisabling) force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
steer_angle_rad = (CS.steeringAngle - self.sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD
# controlsState # controlsState
dat = messaging.new_message('controlsState') dat = messaging.new_message('controlsState')
dat.valid = CS.canValid dat.valid = CS.canValid
dat.controlsState = { controlsState = dat.controlsState
"alertText1": AM.alert_text_1, controlsState.alertText1 = self.AM.alert_text_1
"alertText2": AM.alert_text_2, controlsState.alertText2 = self.AM.alert_text_2
"alertSize": AM.alert_size, controlsState.alertSize = self.AM.alert_size
"alertStatus": AM.alert_status, controlsState.alertStatus = self.AM.alert_status
"alertBlinkingRate": AM.alert_rate, controlsState.alertBlinkingRate = self.AM.alert_rate
"alertType": AM.alert_type, controlsState.alertType = self.AM.alert_type
"alertSound": AM.audible_alert, controlsState.alertSound = self.AM.audible_alert
"driverMonitoringOn": sm['dMonitoringState'].faceDetected, controlsState.driverMonitoringOn = self.sm['dMonitoringState'].faceDetected
"canMonoTimes": list(CS.canMonoTimes), controlsState.canMonoTimes = list(CS.canMonoTimes)
"planMonoTime": sm.logMonoTime['plan'], controlsState.planMonoTime = self.sm.logMonoTime['plan']
"pathPlanMonoTime": sm.logMonoTime['pathPlan'], controlsState.pathPlanMonoTime = self.sm.logMonoTime['pathPlan']
"enabled": isEnabled(state), controlsState.enabled = self.enabled
"active": isActive(state), controlsState.active = self.active
"vEgo": CS.vEgo, controlsState.vEgo = CS.vEgo
"vEgoRaw": CS.vEgoRaw, controlsState.vEgoRaw = CS.vEgoRaw
"angleSteers": CS.steeringAngle, controlsState.angleSteers = CS.steeringAngle
"curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo), controlsState.curvature = self.VM.calc_curvature(steer_angle_rad, CS.vEgo)
"steerOverride": CS.steeringPressed, controlsState.steerOverride = CS.steeringPressed
"state": state, controlsState.state = self.state
"engageable": not bool(get_events(events, [ET.NO_ENTRY])), controlsState.engageable = not bool(get_events(events, [ET.NO_ENTRY]))
"longControlState": LoC.long_control_state, controlsState.longControlState = self.LoC.long_control_state
"vPid": float(LoC.v_pid), controlsState.vPid = float(self.LoC.v_pid)
"vCruise": float(v_cruise_kph), controlsState.vCruise = float(self.v_cruise_kph)
"upAccelCmd": float(LoC.pid.p), controlsState.upAccelCmd = float(self.LoC.pid.p)
"uiAccelCmd": float(LoC.pid.i), controlsState.uiAccelCmd = float(self.LoC.pid.i)
"ufAccelCmd": float(LoC.pid.f), controlsState.ufAccelCmd = float(self.LoC.pid.f)
"angleSteersDes": float(LaC.angle_steers_des), controlsState.angleSteersDes = float(self.LaC.angle_steers_des)
"vTargetLead": float(v_acc), controlsState.vTargetLead = float(v_acc)
"aTarget": float(a_acc), controlsState.aTarget = float(a_acc)
"jerkFactor": float(sm['plan'].jerkFactor), controlsState.jerkFactor = float(self.sm['plan'].jerkFactor)
"gpsPlannerActive": sm['plan'].gpsPlannerActive, controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive
"vCurvature": sm['plan'].vCurvature, controlsState.vCurvature = self.sm['plan'].vCurvature
"decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model, controlsState.decelForModel = self.sm['plan'].longitudinalPlanSource == LongitudinalPlanSource.model
"cumLagMs": -rk.remaining * 1000., controlsState.cumLagMs = -self.rk.remaining * 1000.
"startMonoTime": int(start_time * 1e9), controlsState.startMonoTime = int(start_time * 1e9)
"mapValid": sm['plan'].mapValid, controlsState.mapValid = self.sm['plan'].mapValid
"forceDecel": bool(force_decel), controlsState.forceDecel = bool(force_decel)
"canErrorCounter": can_error_counter, controlsState.canErrorCounter = self.can_error_counter
}
if self.CP.lateralTuning.which() == 'pid':
if CP.lateralTuning.which() == 'pid': controlsState.lateralControlState.pidState = lac_log
dat.controlsState.lateralControlState.pidState = lac_log elif self.CP.lateralTuning.which() == 'lqr':
elif CP.lateralTuning.which() == 'lqr': controlsState.lateralControlState.lqrState = lac_log
dat.controlsState.lateralControlState.lqrState = lac_log elif self.CP.lateralTuning.which() == 'indi':
elif CP.lateralTuning.which() == 'indi': controlsState.lateralControlState.indiState = lac_log
dat.controlsState.lateralControlState.indiState = lac_log self.pm.send('controlsState', dat)
pm.send('controlsState', dat)
# carState # carState
cs_send = messaging.new_message('carState') cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid cs_send.valid = CS.canValid
cs_send.carState = CS cs_send.carState = CS
cs_send.carState.events = events cs_send.carState.events = events
pm.send('carState', cs_send) self.pm.send('carState', cs_send)
# carEvents - logged every second or on change # carEvents - logged every second or on change
events_bytes = events_to_bytes(events) events_bytes = events_to_bytes(events)
if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev): if (self.sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != self.events_prev):
ce_send = messaging.new_message('carEvents', len(events)) ce_send = messaging.new_message('carEvents', len(events))
ce_send.carEvents = events ce_send.carEvents = events
pm.send('carEvents', ce_send) self.pm.send('carEvents', ce_send)
self.events_prev = events_bytes
# carParams - logged every 50 seconds (> 1 per segment) # carParams - logged every 50 seconds (> 1 per segment)
if (sm.frame % int(50. / DT_CTRL) == 0): if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams') cp_send = messaging.new_message('carParams')
cp_send.carParams = CP cp_send.carParams = self.CP
pm.send('carParams', cp_send) self.pm.send('carParams', cp_send)
# carControl # carControl
cc_send = messaging.new_message('carControl') cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid cc_send.valid = CS.canValid
cc_send.carControl = CC cc_send.carControl = CC
pm.send('carControl', cc_send) self.pm.send('carControl', cc_send)
return CC, events_bytes
def controlsd_thread(sm=None, pm=None, can_sock=None): # copy CarControl to pass to CarInterface on the next iteration
gc.disable() self.CC = CC
# start the loop
set_realtime_priority(3)
params = Params()
is_metric = params.get("IsMetric", encoding='utf8') == "1"
is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
passive = passive or not openpilot_enabled_toggle def step(self):
# Passive if internet needed
internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None
passive = passive or internet_needed
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams'])
if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \
'model'])
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(can_sock)
CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
community_feature_disallowed = CP.communityFeature and not community_feature_toggle
read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
CC = car.CarControl.new_message()
AM = AlertManager()
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
elif CP.lateralTuning.which() == 'indi':
LaC = LatControlINDI(CP)
elif CP.lateralTuning.which() == 'lqr':
LaC = LatControlLQR(CP)
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
mismatch_counter = 0
can_error_counter = 0
last_blinker_frame = 0
saturated_count = 0
events_prev = []
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
sm['thermal'].freeSpace = 1.
sm['dMonitoringState'].events = []
sm['dMonitoringState'].awarenessStatus = 1.
sm['dMonitoringState'].faceDetected = False
# detect sound card presence
sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
prof = Profiler(False) # off by default
while True:
start_time = sec_since_boot() start_time = sec_since_boot()
prof.checkpoint("Ratekeeper", ignore=True) self.prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events # Sample data from sockets and get a carState
CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params) CS = self.data_sample()
prof.checkpoint("Sample") self.prof.checkpoint("Sample")
# Create alerts events = self.create_events(CS)
if not sm.alive['plan'] and sm.alive['pathPlan']: # only plan not being received: radar not communicating
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif not sm.all_alive_and_valid():
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None:
events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['pathPlan'].posenetValid:
events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
if not sm['plan'].radarValid:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sounds_available:
events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
if internet_needed:
events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
if community_feature_disallowed:
events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT]))
if read_only and not passive:
events.append(create_event('carUnrecognized', [ET.PERMANENT]))
if log.HealthData.FaultType.relayMalfunction in sm['health'].faults:
events.append(create_event('relayMalfunction', [ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE]))
if not self.read_only:
# Only allow engagement with brake pressed when stopped behind another stopped car # Update control state
if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: self.state_transition(CS, events)
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) self.prof.checkpoint("State transition")
if not read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC) # Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame, saturated_count = \ actuators, v_acc, a_acc, lac_log = self.state_control(CS, events)
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, saturated_count)
prof.checkpoint("State Control") self.prof.checkpoint("State Control")
# Publish data # Publish data
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, LaC, self.publish_logs(CS, events, start_time, actuators, v_acc, a_acc, lac_log)
LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, self.prof.checkpoint("Sent")
is_ldw_enabled, can_error_counter)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
def controlsd_thread(self):
while True:
self.step()
self.rk.monitor_time()
self.prof.display()
def main(sm=None, pm=None, logcan=None): def main(sm=None, pm=None, logcan=None):
controlsd_thread(sm, pm, logcan) controls = Controls(sm, pm, logcan)
controls.controlsd_thread()
if __name__ == "__main__": if __name__ == "__main__":

10
selfdrive/controls/lib/latcontrol_indi.py
Unescape View File

@ -62,9 +62,9 @@ class LatControlINDI():
return self.sat_count > self.sat_limit return self.sat_count > self.sat_limit
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): def update(self, active, CS, CP, path_plan):
# Update Kalman filter # Update Kalman filter
y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]]) y = np.matrix([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message() indi_log = log.ControlsState.LateralINDIState.new_message()
@ -72,7 +72,7 @@ class LatControlINDI():
indi_log.steerRate = math.degrees(self.x[1]) indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2]) indi_log.steerAccel = math.degrees(self.x[2])
if v_ego < 0.3 or not active: if CS.vEgo < 0.3 or not active:
indi_log.active = False indi_log.active = False
self.output_steer = 0.0 self.output_steer = 0.0
self.delayed_output = 0.0 self.delayed_output = 0.0
@ -105,7 +105,7 @@ class LatControlINDI():
else: else:
self.output_steer = self.delayed_output + delta_u self.output_steer = self.delayed_output + delta_u
steers_max = get_steer_max(CP, v_ego) steers_max = get_steer_max(CP, CS.vEgo)
self.output_steer = clip(self.output_steer, -steers_max, steers_max) self.output_steer = clip(self.output_steer, -steers_max, steers_max)
indi_log.active = True indi_log.active = True
@ -116,7 +116,7 @@ class LatControlINDI():
indi_log.delta = float(delta_u) indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer) indi_log.output = float(self.output_steer)
check_saturation = (v_ego > 10.) and not rate_limited and not steer_override check_saturation = (CS.vEgo> 10.) and not CS.steeringRateLimited and not CS.steeringPressed
indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
return float(self.output_steer), float(self.angle_steers_des), indi_log return float(self.output_steer), float(self.angle_steers_des), indi_log

20
selfdrive/controls/lib/latcontrol_lqr.py
Unescape View File

@ -43,22 +43,24 @@ class LatControlLQR():
return self.sat_count > self.sat_limit return self.sat_count > self.sat_limit
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): def update(self, active, CS, CP, path_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message() lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, v_ego) steers_max = get_steer_max(CP, CS.vEgo)
torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed
steering_angle = CS.steeringAngle
# Subtract offset. Zero angle should correspond to zero torque # Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
angle_steers -= path_plan.angleOffset steering_angle -= path_plan.angleOffset
# Update Kalman filter # Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat)) angle_steers_k = float(self.C.dot(self.x_hat))
e = angle_steers - angle_steers_k e = steering_angle - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(eps_torque / torque_scale) + self.L.dot(e) self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
if v_ego < 0.3 or not active: if CS.vEgo < 0.3 or not active:
lqr_log.active = False lqr_log.active = False
lqr_output = 0. lqr_output = 0.
self.reset() self.reset()
@ -70,7 +72,7 @@ class LatControlLQR():
lqr_output = torque_scale * u_lqr / self.scale lqr_output = torque_scale * u_lqr / self.scale
# Integrator # Integrator
if steer_override: if CS.steeringPressed:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else: else:
error = self.angle_steers_des - angle_steers_k error = self.angle_steers_des - angle_steers_k
@ -84,7 +86,7 @@ class LatControlLQR():
self.output_steer = lqr_output + self.i_lqr self.output_steer = lqr_output + self.i_lqr
self.output_steer = clip(self.output_steer, -steers_max, steers_max) self.output_steer = clip(self.output_steer, -steers_max, steers_max)
check_saturation = (v_ego > 10) and not rate_limited and not steer_override check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset

18
selfdrive/controls/lib/latcontrol_pid.py
Unescape View File

@ -14,31 +14,31 @@ class LatControlPID():
def reset(self): def reset(self):
self.pid.reset() self.pid.reset()
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): def update(self, active, CS, CP, path_plan):
pid_log = log.ControlsState.LateralPIDState.new_message() pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers) pid_log.steerAngle = float(CS.steeringAngle)
pid_log.steerRate = float(angle_steers_rate) pid_log.steerRate = float(CS.steeringRate)
if v_ego < 0.3 or not active: if CS.vEgo < 0.3 or not active:
output_steer = 0.0 output_steer = 0.0
pid_log.active = False pid_log.active = False
self.pid.reset() self.pid.reset()
else: else:
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
steers_max = get_steer_max(CP, v_ego) steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque: if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers # TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0 deadzone = 0.0
check_saturation = (v_ego > 10) and not rate_limited and not steer_override check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=check_saturation, override=steer_override, output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True pid_log.active = True
pid_log.p = self.pid.p pid_log.p = self.pid.p
pid_log.i = self.pid.i pid_log.i = self.pid.i

20
selfdrive/controls/lib/longcontrol.py
Unescape View File

@ -71,19 +71,19 @@ class LongControl():
self.pid.reset() self.pid.reset()
self.v_pid = v_pid self.v_pid = v_pid
def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP): def update(self, active, CS, v_target, v_target_future, a_target, CP):
"""Update longitudinal control. This updates the state machine and runs a PID loop""" """Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits # Actuation limits
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV)
brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV)
# Update state machine # Update state machine
output_gb = self.last_output_gb output_gb = self.last_output_gb
self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego, self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
v_target_future, self.v_pid, output_gb, v_target_future, self.v_pid, output_gb,
brake_pressed, cruise_standstill) CS.brakePressed, CS.cruiseState.standstill)
v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off: if self.long_control_state == LongCtrlState.off:
self.v_pid = v_ego_pid self.v_pid = v_ego_pid
@ -98,7 +98,7 @@ class LongControl():
# Toyota starts braking more when it thinks you want to stop # Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7 prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
@ -109,18 +109,18 @@ class LongControl():
# Intention is to stop, switch to a different brake control until we stop # Intention is to stop, switch to a different brake control until we stop
elif self.long_control_state == LongCtrlState.stopping: elif self.long_control_state == LongCtrlState.stopping:
# Keep applying brakes until the car is stopped # Keep applying brakes until the car is stopped
if not standstill or output_gb > -BRAKE_STOPPING_TARGET: if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET:
output_gb -= STOPPING_BRAKE_RATE / RATE output_gb -= STOPPING_BRAKE_RATE / RATE
output_gb = clip(output_gb, -brake_max, gas_max) output_gb = clip(output_gb, -brake_max, gas_max)
self.v_pid = v_ego self.v_pid = CS.vEgo
self.pid.reset() self.pid.reset()
# Intention is to move again, release brake fast before handing control to PID # Intention is to move again, release brake fast before handing control to PID
elif self.long_control_state == LongCtrlState.starting: elif self.long_control_state == LongCtrlState.starting:
if output_gb < -0.2: if output_gb < -0.2:
output_gb += STARTING_BRAKE_RATE / RATE output_gb += STARTING_BRAKE_RATE / RATE
self.v_pid = v_ego self.v_pid = CS.vEgo
self.pid.reset() self.pid.reset()
self.last_output_gb = output_gb self.last_output_gb = output_gb

2
selfdrive/test/profiling/controlsd.py
Unescape View File

@ -7,7 +7,7 @@ import pprofile
import pyprof2calltree import pyprof2calltree
from tools.lib.logreader import LogReader from tools.lib.logreader import LogReader
from selfdrive.controls.controlsd import controlsd_thread from selfdrive.controls.controlsd import main as controlsd_thread
from selfdrive.test.profiling.lib import SubMaster, PubMaster, SubSocket, ReplayDone from selfdrive.test.profiling.lib import SubMaster, PubMaster, SubSocket, ReplayDone
from selfdrive.test.process_replay.process_replay import CONFIGS from selfdrive.test.process_replay.process_replay import CONFIGS

Write Preview
Loading…
Cancel
Save