#!/usr/bin/env python3
import os
import time
import cereal.messaging as messaging
from cereal import car
from panda import ALTERNATIVE_EXPERIENCE
from openpilot.common.params import Params
from openpilot.common.realtime import 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
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.controls.lib.events import Events
REPLAY = "REPLAY" in os.environ
EventName = car.CarEvent.EventName
[docs]
class CarD:
CI: CarInterfaceBase
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', '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.last_actuators = None
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
# set alternative experiences from parameters
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
self.CP.alternativeExperience = 0
if not self.disengage_on_accelerator:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly
self.CP.passive = not controller_available or self.CP.dashcamOnly
if self.CP.passive:
safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config]
# Write previous route's CarParams
prev_cp = self.params.get("CarParamsPersistent")
if prev_cp is not None:
self.params.put("CarParamsPrevRoute", prev_cp)
# Write CarParams for controls and radard
cp_bytes = self.CP.to_bytes()
self.params.put("CarParams", cp_bytes)
self.params.put_nonblocking("CarParamsCache", cp_bytes)
self.params.put_nonblocking("CarParamsPersistent", cp_bytes)
self.CS_prev = car.CarState.new_message()
self.events = Events()
[docs]
def initialize(self):
"""Initialize CarInterface, once controls are ready"""
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
[docs]
def state_update(self) -> car.CarState:
"""carState update loop, driven by can"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC_prev, 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
self.update_events(CS)
self.state_publish(CS)
CS = CS.as_reader()
self.CS_prev = CS
return CS
[docs]
def update_events(self, CS: car.CarState) -> car.CarState:
self.events.clear()
self.events.add_from_msg(CS.events)
# Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
self.events.add(EventName.pedalPressed)
CS.events = self.events.to_msg()
[docs]
def state_publish(self, CS: car.CarState):
"""carState and carParams publish loop"""
# carState
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
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)
# publish new carOutput
co_send = messaging.new_message('carOutput')
co_send.valid = True
if self.last_actuators is not None:
co_send.carOutput.actuatorsOutput = self.last_actuators
self.pm.send('carOutput', co_send)
[docs]
def controls_update(self, CS: car.CarState, 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)
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.CC_prev = CC