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

standalone process

Browse Source
pull/32380/head
Shane Smiskol 1 year ago
parent 3f9f33d25f
commit e2d62fa7b5
4 changed files with 58 additions and 28 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. 2
      cereal
  2. 47
      selfdrive/car/card.py
  3. 36
      selfdrive/controls/controlsd.py
  4. 1
      selfdrive/manager/process_config.py

2
cereal

@ -1 +1 @@
Subproject commit 84af7ef6654b2e810a2871b8ddd27ad170696599
Subproject commit c2adf2fbc460b3854334a9430b918565547b3757

47
selfdrive/car/card.py
Unescape View File

@ -9,7 +9,7 @@ from cereal import car
from panda import ALTERNATIVE_EXPERIENCE
from openpilot.common.params import Params
from openpilot.common.realtime import DT_CTRL
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
from openpilot.selfdrive.car.car_helpers import get_car, get_one_can
@ -18,19 +18,20 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase
REPLAY = "REPLAY" in os.environ
class CarD:
class Car:
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.sm = messaging.SubMaster(['pandaStates', 'carControl', 'controlsState'])
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput'])
self.can_rcv_timeout_counter = 0 # consecutive timeout count
self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count
self.CC_prev = car.CarControl.new_message()
self.CoS_prev = car.CarState.new_message()
self.last_actuators = None
@ -75,9 +76,8 @@ class CarD:
self.params.put_nonblocking("CarParamsCache", cp_bytes)
self.params.put_nonblocking("CarParamsPersistent", cp_bytes)
def initialize(self):
"""Initialize CarInterface, once controls are ready"""
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
# card is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
def state_update(self):
"""carState update loop, driven by can"""
@ -102,10 +102,6 @@ class CarD:
if can_rcv_valid and REPLAY:
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
self.state_publish()
return self.CS
def state_publish(self):
"""carState and carParams publish loop"""
@ -113,6 +109,9 @@ class CarD:
cs_send = messaging.new_message('carState')
cs_send.valid = self.CS.canValid
cs_send.carState = self.CS
cs_send.canRcvTimeout = self.can_rcv_timeout
cs_send.canErrorCounter = self.can_rcv_cum_timeout_counter
cs_send.cumLagMs = -self.rk.remaining * 1000.
self.pm.send('carState', cs_send)
# carParams - logged every 50 seconds (> 1 per segment)
@ -138,3 +137,31 @@ class CarD:
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid))
self.CC_prev = CC
def step(self):
if self.sm['controlsState'].initialized and not self.CoS_prev.initialized:
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
self.state_update()
self.state_publish()
if not self.CP.passive and self.sm['controlsState'].initialized:
self.controls_update(self.sm['carControl'])
self.CoS_prev = self.sm['controlsState']
def card_thread(self):
while True:
self.step()
self.rk.monitor_time()
def main():
config_realtime_process(4, Priority.CTRL_HIGH)
car = Car()
car.card_thread()
if __name__ == "__main__":
main()

36
selfdrive/controls/controlsd.py
Unescape View File

@ -18,8 +18,7 @@ from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.car.car_helpers import get_startup_event
from openpilot.selfdrive.car.card import CarD
from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event
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
@ -61,16 +60,16 @@ ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES)
class Controls:
def __init__(self, CI=None):
self.card = CarD(CI)
self.params = Params()
with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg:
# TODO: this shouldn't need to be a builder
self.CP = msg.as_builder()
self.CI = self.card.CI
# TODO: if we want to remove usage of CI, we need to move controllers and other fields to card (like curvature & desiredCurvature)
# TODO: we need to ensure no CI calls here updates any state that we expect to be used to change sendcan over in card, because it won't
# TODO: perhaps there's a different attribute or object that just provides car-specific functions for long and lat
self.CI = get_car_interface(self.CP)
# Ensure the current branch is cached, otherwise the first iteration of controlsd lags
self.branch = get_short_branch()
@ -89,7 +88,8 @@ class Controls:
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'testJoystick'] + self.camera_packets + self.sensor_packets,
'carState', 'testJoystick'] + self.camera_packets + self.sensor_packets,
poll='carState',
ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
frequency=int(1/DT_CTRL))
@ -165,7 +165,7 @@ class Controls:
elif self.CP.passive:
self.events.add(EventName.dashcamMode, static=True)
# controlsd is driven by can recv, expected at 100Hz
# controlsd is driven by carState, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None)
def set_initial_state(self):
@ -324,7 +324,7 @@ class Controls:
# generic catch-all. ideally, a more specific event should be added above instead
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 self.card.can_rcv_timeout) and no_system_errors:
if (not self.sm.all_checks() or CS.canRcvTimeout) and no_system_errors:
if not self.sm.all_alive():
self.events.add(EventName.commIssue)
elif not self.sm.all_freq_ok():
@ -336,7 +336,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': self.card.can_rcv_timeout,
'can_rcv_timeout': CS.canRcvTimeout,
}
if logs != self.logged_comm_issue:
cloudlog.event("commIssue", error=True, **logs)
@ -398,9 +398,10 @@ class Controls:
def data_sample(self):
"""Receive data from sockets and update carState"""
CS = self.card.state_update()
# TODO: is this right?
self.sm.update(20)
self.sm.update(0)
CS = self.sm['carState']
if not self.initialized:
all_valid = CS.canValid and self.sm.all_checks()
@ -412,8 +413,8 @@ class Controls:
if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams:
self.sm.ignore_alive.append('wideRoadCameraState')
if not self.CP.passive:
self.card.initialize()
# if not self.CP.passive:
# self.card.initialize()
self.initialized = True
self.set_initial_state()
@ -439,7 +440,7 @@ class Controls:
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
return CS
@ -727,7 +728,7 @@ class Controls:
hudControl.visualAlert = current_alert.visual_alert
if not self.CP.passive and self.initialized:
self.card.controls_update(CC)
# self.card.controls_update(CC)
self.last_actuators = CO.actuatorsOutput
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
@ -775,9 +776,10 @@ class Controls:
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter
# controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter
controlsState.experimentalMode = self.experimental_mode
controlsState.personality = self.personality
controlsState.initialized = self.initialized
lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode:

1
selfdrive/manager/process_config.py
Unescape View File

@ -64,6 +64,7 @@ procs = [
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad),
PythonProcess("card", "selfdrive.car.card", only_onroad),
PythonProcess("deleter", "system.loggerd.deleter", always_run),
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)),
PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI),

Write Preview
Loading…
Cancel
Save