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

Split out can control into new "card" class (#31529)

Browse Source 
* card v1

* fix car events

* fix proc replay

* lets keep that the same

* no extra space here

* move can recv timeouts to card

* organize imports

* organize imports

* slightly bump cpu

* not a card!
pull/31532/head
Justin Newberry 1 year ago committed by GitHub
parent 0eba392cf2
commit 837b811f9c
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 102 additions and 51 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 151
      selfdrive/controls/controlsd.py
  2. 2
      selfdrive/test/test_onroad.py

151
selfdrive/controls/controlsd.py
Unescape View File

@ -20,6 +20,7 @@ from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature
from openpilot.selfdrive.controls.lib.events import Events, ET
@ -60,22 +61,111 @@ ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES)
class CarD:
CI: CarInterfaceBase
CS: car.CarState
def __init__(self, CI=None):
self.can_sock = messaging.sub_sock('can', timeout=20)
self.sm = messaging.SubMaster(['pandaStates'])
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams'])
self.can_rcv_timeout_counter = 0 # conseuctive timeout count
self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count
self.params = Params()
if CI is None:
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates)
experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled")
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas)
else:
self.CI, self.CP = CI, CI.CP
def initialize(self):
"""Initialize CarInterface, once controls are ready"""
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
def state_update(self, CC: car.CarControl):
"""carState update loop, driven by can"""
# TODO: This should not depend on carControl
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
self.CS = self.CI.update(CC, can_strs)
self.sm.update(0)
can_rcv_valid = len(can_strs) > 0
# Check for CAN timeout
if not can_rcv_valid:
self.can_rcv_timeout_counter += 1
self.can_rcv_cum_timeout_counter += 1
else:
self.can_rcv_timeout_counter = 0
self.can_rcv_timeout = self.can_rcv_timeout_counter >= 5
if can_rcv_valid and REPLAY:
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
return self.CS
def state_publish(self, car_events):
"""carState and carParams publish loop"""
# TODO: carState should be independent of the event loop
# carState
cs_send = messaging.new_message('carState')
cs_send.valid = self.CS.canValid
cs_send.carState = self.CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.valid = True
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
def controls_update(self, CC: car.CarControl):
"""control update loop, driven by carControl"""
# send car controls over can
now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
actuators_output, can_sends = self.CI.apply(CC, now_nanos)
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid))
return actuators_output
class Controls:
def __init__(self, CI=None):
self.card = CarD(CI)
self.CP = self.card.CP
self.CI = self.card.CI
config_realtime_process(4, Priority.CTRL_HIGH)
# Ensure the current branch is cached, otherwise the first iteration of controlsd lags
self.branch = get_short_branch()
# Setup sockets
self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
'carControl', 'onroadEvents', 'carParams'])
self.pm = messaging.PubMaster(['controlsState', 'carControl', 'onroadEvents'])
self.sensor_packets = ["accelerometer", "gyroscope"]
self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
self.log_sock = messaging.sub_sock('androidLog')
self.can_sock = messaging.sub_sock('can', timeout=20)
self.params = Params()
ignore = self.sensor_packets + ['testJoystick']
@ -88,17 +178,6 @@ class Controls:
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
frequency=int(1/DT_CTRL))
if CI is None:
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates)
experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled")
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas)
else:
self.CI, self.CP = CI, CI.CP
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
# set alternative experiences from parameters
@ -164,8 +243,6 @@ class Controls:
self.soft_disable_timer = 0
self.mismatch_counter = 0
self.cruise_mismatch_counter = 0
self.can_rcv_timeout_counter = 0 # conseuctive timeout count
self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count
self.last_blinker_frame = 0
self.last_steering_pressed_frame = 0
self.distance_traveled = 0
@ -353,10 +430,9 @@ class Controls:
self.events.add(EventName.canError)
# 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.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_checks() or self.card.can_rcv_timeout) and no_system_errors:
if not self.sm.all_alive():
self.events.add(EventName.commIssue)
elif not self.sm.all_freq_ok():
@ -368,7 +444,7 @@ class Controls:
'invalid': [s for s, valid in self.sm.valid.items() if not valid],
'not_alive': [s for s, alive in self.sm.alive.items() if not alive],
'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
'can_rcv_timeout': can_rcv_timeout,
'can_rcv_timeout': self.card.can_rcv_timeout,
}
if logs != self.logged_comm_issue:
cloudlog.event("commIssue", error=True, **logs)
@ -430,11 +506,7 @@ class Controls:
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)
if len(can_strs) and REPLAY:
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
CS = self.card.state_update(self.CC)
self.sm.update(0)
@ -449,7 +521,7 @@ class Controls:
self.sm.ignore_alive.append('wideRoadCameraState')
if not self.CP.passive:
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
self.card.initialize()
self.initialized = True
self.set_initial_state()
@ -466,13 +538,6 @@ class Controls:
error=True,
)
# Check for CAN timeout
if not can_strs:
self.can_rcv_timeout_counter += 1
self.can_rcv_cum_timeout_counter += 1
else:
self.can_rcv_timeout_counter = 0
# When the panda and controlsd do not agree on controls_allowed
# 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.
@ -761,10 +826,7 @@ class Controls:
hudControl.visualAlert = current_alert.visual_alert
if not self.CP.passive and self.initialized:
# send car controls over can
now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
self.last_actuators, can_sends = self.CI.apply(CC, now_nanos)
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
self.last_actuators = self.card.controls_update(CC)
CC.actuatorsOutput = self.last_actuators
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.steer_limited = abs(CC.actuators.steeringAngleDeg - CC.actuatorsOutput.steeringAngleDeg) > \
@ -812,7 +874,7 @@ class Controls:
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_rcv_cum_timeout_counter
controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter
controlsState.experimentalMode = self.experimental_mode
lat_tuning = self.CP.lateralTuning.which()
@ -827,13 +889,9 @@ class Controls:
self.pm.send('controlsState', dat)
# carState
car_events = self.events.to_msg()
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
self.card.state_publish(car_events)
# onroadEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
@ -843,13 +901,6 @@ class Controls:
self.pm.send('onroadEvents', ce_send)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.valid = True
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid

2
selfdrive/test/test_onroad.py
Unescape View File

@ -29,7 +29,7 @@ from openpilot.tools.lib.logreader import LogReader
# Baseline CPU usage by process
PROCS = {
"selfdrive.controls.controlsd": 39.0,
"selfdrive.controls.controlsd": 41.0,
"./loggerd": 14.0,
"./encoderd": 17.0,
"./camerad": 14.5,

Write Preview
Loading…
Cancel
Save