mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-12 12:52:13 +08:00
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!
old-commit-hash: 837b811f9c78628a1c0ba50f9f287bf96e39144d
This commit is contained in:
Justin Newberry
2024-02-21 16:18:43 -05:00
committed by GitHub
parent 0d6c16e771
commit 8256969d40
2 changed files with 111 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+110 -59
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,33 +61,19 @@ ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES)
class Controls:
class CarD:
CI: CarInterfaceBase
CS: car.CarState
def __init__(self, CI=None):
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.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.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()
ignore = self.sensor_packets + ['testJoystick']
if SIMULATION:
ignore += ['driverCameraState', 'managerState']
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'testJoystick'] + self.camera_packets + self.sensor_packets,
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
@@ -99,6 +86,98 @@ class Controls:
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(['controlsState', 'carControl', 'onroadEvents'])
self.sensor_packets = ["accelerometer", "gyroscope"]
self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
self.log_sock = messaging.sub_sock('androidLog')
self.params = Params()
ignore = self.sensor_packets + ['testJoystick']
if SIMULATION:
ignore += ['driverCameraState', 'managerState']
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'testJoystick'] + self.camera_packets + self.sensor_packets,
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
frequency=int(1/DT_CTRL))
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
selfdrive/test/test_onroad.py
+1 -1
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,