diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 38fe5a66de..b38a7dedae 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -24,7 +24,7 @@ class Car: def __init__(self, CI=None): self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'controlsState'], poll='carControl') + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'controlsState']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput']) self.can_rcv_timeout_counter = 0 # consecutive timeout count @@ -85,6 +85,8 @@ class Car: 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 @@ -99,19 +101,6 @@ class Car: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime - # carState is sent last to ensure controlsd receives above services this cycle - cs_send = messaging.new_message('carState') - cs_send.valid = CS.canValid - cs_send.carState = CS - cs_send.carState.canRcvTimeout = self.can_rcv_timeout - cs_send.carState.canErrorCounter = self.can_rcv_cum_timeout_counter - cs_send.carState.cumLagMs = -self.rk.remaining * 1000. - self.pm.send('carState', cs_send) - cloudlog.timestamp('Sent carState') - - # Wait for carControl response from controlsd - self.sm.update(20) - return CS def state_publish(self, CS: car.CarState): @@ -130,14 +119,15 @@ class Car: co_send.carOutput.actuatorsOutput = self.last_actuators_output self.pm.send('carOutput', co_send) - # # carState is sent last to ensure controlsd receives above services this cycle - # cs_send = messaging.new_message('carState') - # cs_send.valid = CS.canValid - # cs_send.carState = CS - # cs_send.carState.canRcvTimeout = self.can_rcv_timeout - # cs_send.carState.canErrorCounter = self.can_rcv_cum_timeout_counter - # cs_send.carState.cumLagMs = -self.rk.remaining * 1000. - # self.pm.send('carState', cs_send) + # kick off controlsd step now while we actuate the latest carControl packet + cs_send = messaging.new_message('carState') + cs_send.valid = CS.canValid + cs_send.carState = CS + cs_send.carState.canRcvTimeout = self.can_rcv_timeout + cs_send.carState.canErrorCounter = self.can_rcv_cum_timeout_counter + cs_send.carState.cumLagMs = -self.rk.remaining * 1000. + self.pm.send('carState', cs_send) + cloudlog.timestamp('Sent carState') def controls_update(self, CS: car.CarState, CC: car.CarControl): """control update loop, driven by carControl""" @@ -160,14 +150,13 @@ class Car: CS = self.state_update() cloudlog.timestamp("State updated") + self.state_publish(CS) + cloudlog.timestamp("State published") + if not self.CP.passive and controlsState.initialized: self.controls_update(CS, self.sm['carControl']) cloudlog.timestamp("Controls updated") - # TODO: this should be moved back up, note that carOutput will be delayed a frame - self.state_publish(CS) - cloudlog.timestamp("State published") - self.controlsState_prev = controlsState def card_thread(self): diff --git a/tools/latencylogger/plot_timestamps.py b/tools/latencylogger/plot_timestamps.py index 8f4701f382..4a7f5af841 100755 --- a/tools/latencylogger/plot_timestamps.py +++ b/tools/latencylogger/plot_timestamps.py @@ -131,8 +131,8 @@ if __name__ == "__main__": # r = DEMO_ROUTE if args.demo else args.route_or_segment_name.strip() # lr = LogReader(r, sort_by_time=True) - # lr = LogReader('08e4c2a99df165b1/00000016--c3a4ca99ec/0', sort_by_time=True) # normal - lr = LogReader('08e4c2a99df165b1/00000017--e2d24ab118/0', sort_by_time=True) # polls on carControl + lr = LogReader('08e4c2a99df165b1/00000016--c3a4ca99ec/0', sort_by_time=True) # normal + # lr = LogReader('08e4c2a99df165b1/00000017--e2d24ab118/0', sort_by_time=True) # polls on carControl # lr = LogReader('08e4c2a99df165b1/00000018--cf65e47c24/0', sort_by_time=True) # polls on carControl, sends it earlier # lr = LogReader('08e4c2a99df165b1/00000019--e73e3ab4df/0', sort_by_time=True) # polls on carControl, more logging