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

no class

Browse Source
pull/32380/head
Shane Smiskol 11 months ago
parent f3146763cf
commit 5499b83c2d
1 changed files with 91 additions and 107 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. 198
      selfdrive/car/card.py

198
selfdrive/car/card.py
Unescape View File

@ -22,170 +22,154 @@ State = log.ControlsState.OpenpilotState
EventName = car.CarEvent.EventName EventName = car.CarEvent.EventName
class Car: def loop(CI=None):
CI: CarInterfaceBase can_sock = messaging.sub_sock('can', timeout=20)
sm = messaging.SubMaster(['pandaStates', 'carControl', 'controlsState'])
pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput'])
def __init__(self, CI=None): can_rcv_timeout_counter = 0 # consecutive timeout count
self.can_sock = messaging.sub_sock('can', timeout=20) can_rcv_cum_timeout_counter = 0 # cumulative timeout count
self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'controlsState'])
self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput'])
self.can_rcv_timeout_counter = 0 # consecutive timeout count CC_prev = car.CarControl.new_message()
self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count CS_prev = car.CarState.new_message()
controlsState_prev = car.CarState.new_message()
self.CC_prev = car.CarControl.new_message() last_actuators_output = car.CarControl.Actuators.new_message()
self.CS_prev = car.CarState.new_message()
self.controlsState_prev = car.CarState.new_message()
self.last_actuators_output = car.CarControl.Actuators.new_message() params = Params()
params = Params() if CI is None:
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(can_sock)
if CI is None: num_pandas = len(messaging.recv_one_retry(sm.sock['pandaStates']).pandaStates)
# wait for one pandaState and one CAN packet experimental_long_allowed = params.get_bool("ExperimentalLongitudinalEnabled")
print("Waiting for CAN messages...") CI, CP = get_car(can_sock, pm.sock['sendcan'], experimental_long_allowed, num_pandas)
get_one_can(self.can_sock) else:
CI, CP = CI, CI.CP
num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) # set alternative experiences from parameters
experimental_long_allowed = params.get_bool("ExperimentalLongitudinalEnabled") disengage_on_accelerator = params.get_bool("DisengageOnAccelerator")
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) CP.alternativeExperience = 0
else: if not disengage_on_accelerator:
self.CI, self.CP = CI, CI.CP CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
# set alternative experiences from parameters
self.disengage_on_accelerator = 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 = params.get_bool("OpenpilotEnabledToggle") openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle")
controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly controller_available = CI.CC is not None and openpilot_enabled_toggle and not CP.dashcamOnly
self.CP.passive = not controller_available or self.CP.dashcamOnly CP.passive = not controller_available or CP.dashcamOnly
if self.CP.passive: if CP.passive:
safety_config = car.CarParams.SafetyConfig.new_message() safety_config = car.CarParams.SafetyConfig.new_message()
safety_config.safetyModel = car.CarParams.SafetyModel.noOutput safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
self.CP.safetyConfigs = [safety_config] CP.safetyConfigs = [safety_config]
# Write previous route's CarParams # Write previous route's CarParams
prev_cp = params.get("CarParamsPersistent") prev_cp = params.get("CarParamsPersistent")
if prev_cp is not None: if prev_cp is not None:
params.put("CarParamsPrevRoute", prev_cp) params.put("CarParamsPrevRoute", prev_cp)
# Write CarParams for controls and radard # Write CarParams for controls and radard
cp_bytes = self.CP.to_bytes() cp_bytes = CP.to_bytes()
params.put("CarParams", cp_bytes) params.put("CarParams", cp_bytes)
params.put_nonblocking("CarParamsCache", cp_bytes) params.put_nonblocking("CarParamsCache", cp_bytes)
params.put_nonblocking("CarParamsPersistent", cp_bytes) params.put_nonblocking("CarParamsPersistent", cp_bytes)
self.events = Events() events = Events()
# card is driven by can recv, expected at 100Hz # card is driven by can recv, expected at 100Hz
self.rk = Ratekeeper(100, print_delay_threshold=None) rk = Ratekeeper(100, print_delay_threshold=None)
def state_update(self) -> car.CarState: while True:
# *** state update ***
"""carState update loop, driven by can""" """carState update loop, driven by can"""
# Update carState from CAN # Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
CS = self.CI.update(self.CC_prev, can_strs) CS = CI.update(CC_prev, can_strs)
self.sm.update(0) sm.update(0)
can_rcv_valid = len(can_strs) > 0 can_rcv_valid = len(can_strs) > 0
# Check for CAN timeout # Check for CAN timeout
if not can_rcv_valid: if not can_rcv_valid:
self.can_rcv_timeout_counter += 1 can_rcv_timeout_counter += 1
self.can_rcv_cum_timeout_counter += 1 can_rcv_cum_timeout_counter += 1
else: else:
self.can_rcv_timeout_counter = 0 can_rcv_timeout_counter = 0
self.can_rcv_timeout = self.can_rcv_timeout_counter >= 5 can_rcv_timeout = can_rcv_timeout_counter >= 5
if can_rcv_valid and REPLAY: if can_rcv_valid and REPLAY:
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
return CS # *** update events ***
events.clear()
def update_events(self, CS: car.CarState) -> car.CarState: events.add_from_msg(CS.events)
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 # 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 \ if (CS.gasPressed and not CS_prev.gasPressed and disengage_on_accelerator) or \
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ (CS.brakePressed and (not CS_prev.brakePressed or not CS.standstill)) or \
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): (CS.regenBraking and (not CS_prev.regenBraking or not CS.standstill)):
self.events.add(EventName.pedalPressed) events.add(EventName.pedalPressed)
CS.events = self.events.to_msg() CS.events = events.to_msg()
def state_publish(self, CS: car.CarState): # *** state publish ***
"""carState and carParams publish loop""" """carState and carParams publish loop"""
# carParams - logged every 50 seconds (> 1 per segment) # carParams - logged every 50 seconds (> 1 per segment)
if self.sm.frame % int(50. / DT_CTRL) == 0: if sm.frame % int(50. / DT_CTRL) == 0:
cp_send = messaging.new_message('carParams') cp_send = messaging.new_message('carParams')
cp_send.valid = True cp_send.valid = True
cp_send.carParams = self.CP cp_send.carParams = CP
self.pm.send('carParams', cp_send) pm.send('carParams', cp_send)
# publish new carOutput # publish new carOutput
co_send = messaging.new_message('carOutput') co_send = messaging.new_message('carOutput')
co_send.valid = self.sm.all_checks(['carControl']) co_send.valid = sm.all_checks(['carControl'])
co_send.carOutput.actuatorsOutput = self.last_actuators_output co_send.carOutput.actuatorsOutput = last_actuators_output
self.pm.send('carOutput', co_send) pm.send('carOutput', co_send)
# kick off controlsd step while we actuate the latest carControl packet # kick off controlsd step while we actuate the latest carControl packet
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.canRcvTimeout = self.can_rcv_timeout cs_send.carState.canRcvTimeout = can_rcv_timeout
cs_send.carState.canErrorCounter = self.can_rcv_cum_timeout_counter cs_send.carState.canErrorCounter = can_rcv_cum_timeout_counter
cs_send.carState.cumLagMs = -self.rk.remaining * 1000. cs_send.carState.cumLagMs = -rk.remaining * 1000.
self.pm.send('carState', cs_send) pm.send('carState', cs_send)
def controls_update(self, CS: car.CarState, CC: car.CarControl):
"""control update loop, driven by carControl"""
if not self.controlsState_prev.initialized:
# Initialize CarInterface, once controls are ready
self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
if self.sm.all_checks(['carControl']):
# send car controls over can
now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
self.last_actuators_output, 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
def step(self): # *** controls update ***
CS = self.state_update() controlsState = sm['controlsState']
if not CP.passive and controlsState.initialized:
"""control update loop, driven by carControl"""
self.update_events(CS) if not controlsState_prev.initialized:
# Initialize CarInterface, once controls are ready
CI.init(CP, can_sock, pm.sock['sendcan'])
self.state_publish(CS) if sm.all_checks(['carControl']):
# send car controls over can
CC = sm['carControl']
now_nanos = can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
last_actuators_output, can_sends = CI.apply(CC, now_nanos)
pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
controlsState = self.sm['controlsState'] CC_prev = CC
if not self.CP.passive and controlsState.initialized:
self.controls_update(CS, self.sm['carControl'])
self.controlsState_prev = controlsState controlsState_prev = controlsState
self.CS_prev = CS.as_reader() CS_prev = CS.as_reader()
def card_thread(self): rk.monitor_time()
while True:
self.step()
self.rk.monitor_time()
def main(): def main():
config_realtime_process(4, Priority.CTRL_HIGH) config_realtime_process(4, Priority.CTRL_HIGH)
car = Car() loop()
car.card_thread()
if __name__ == "__main__": if __name__ == "__main__":

Write Preview
Loading…
Cancel
Save