|
|
@ -20,7 +20,6 @@ REPLAY = "REPLAY" in os.environ |
|
|
|
|
|
|
|
|
|
|
|
class CarD: |
|
|
|
class CarD: |
|
|
|
CI: CarInterfaceBase |
|
|
|
CI: CarInterfaceBase |
|
|
|
CS: car.CarState |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def __init__(self, CI=None): |
|
|
|
def __init__(self, CI=None): |
|
|
|
self.can_sock = messaging.sub_sock('can', timeout=20) |
|
|
|
self.can_sock = messaging.sub_sock('can', timeout=20) |
|
|
@ -83,7 +82,7 @@ class CarD: |
|
|
|
|
|
|
|
|
|
|
|
# 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(self.can_sock, wait_for_one=True) |
|
|
|
self.CS = self.CI.update(self.CC_prev, can_strs) |
|
|
|
CS = self.CI.update(self.CC_prev, can_strs) |
|
|
|
|
|
|
|
|
|
|
|
self.sm.update(0) |
|
|
|
self.sm.update(0) |
|
|
|
|
|
|
|
|
|
|
@ -101,21 +100,21 @@ class CarD: |
|
|
|
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 |
|
|
|
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime |
|
|
|
|
|
|
|
|
|
|
|
self.state_publish() |
|
|
|
self.state_publish(CS) |
|
|
|
|
|
|
|
|
|
|
|
return self.CS |
|
|
|
return CS |
|
|
|
|
|
|
|
|
|
|
|
def state_publish(self): |
|
|
|
def state_publish(self, CS: car.CarState): |
|
|
|
"""carState and carParams publish loop""" |
|
|
|
"""carState and carParams publish loop""" |
|
|
|
|
|
|
|
|
|
|
|
# carState |
|
|
|
# carState |
|
|
|
cs_send = messaging.new_message('carState') |
|
|
|
cs_send = messaging.new_message('carState') |
|
|
|
cs_send.valid = self.CS.canValid |
|
|
|
cs_send.valid = CS.canValid |
|
|
|
cs_send.carState = self.CS |
|
|
|
cs_send.carState = CS |
|
|
|
self.pm.send('carState', cs_send) |
|
|
|
self.pm.send('carState', cs_send) |
|
|
|
|
|
|
|
|
|
|
|
# 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 self.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 = self.CP |
|
|
@ -128,12 +127,12 @@ class CarD: |
|
|
|
co_send.carOutput.actuatorsOutput = self.last_actuators |
|
|
|
co_send.carOutput.actuatorsOutput = self.last_actuators |
|
|
|
self.pm.send('carOutput', co_send) |
|
|
|
self.pm.send('carOutput', co_send) |
|
|
|
|
|
|
|
|
|
|
|
def controls_update(self, CC: car.CarControl): |
|
|
|
def controls_update(self, CS: car.CarState, CC: car.CarControl): |
|
|
|
"""control update loop, driven by carControl""" |
|
|
|
"""control update loop, driven by carControl""" |
|
|
|
|
|
|
|
|
|
|
|
# send car controls over can |
|
|
|
# send car controls over can |
|
|
|
now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) |
|
|
|
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.last_actuators, 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)) |
|
|
|
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) |
|
|
|
|
|
|
|
|
|
|
|
self.CC_prev = CC |
|
|
|
self.CC_prev = CC |
|
|
|