forked from Archive/PX4-Autopilot
mavlink_ulog_streaming: when stopping, wait after receiving the ack
So we log additional data send afterwards
This commit is contained in:
parent
e1ac6fe297
commit
d3c45c00c7
|
@ -13,6 +13,7 @@ import datetime
|
||||||
from timeit import default_timer as timer
|
from timeit import default_timer as timer
|
||||||
os.environ['MAVLINK20'] = '1' # The commands require mavlink 2
|
os.environ['MAVLINK20'] = '1' # The commands require mavlink 2
|
||||||
from argparse import ArgumentParser
|
from argparse import ArgumentParser
|
||||||
|
import signal
|
||||||
|
|
||||||
try:
|
try:
|
||||||
from pymavlink import mavutil
|
from pymavlink import mavutil
|
||||||
|
@ -24,6 +25,8 @@ except ImportError as e:
|
||||||
print("")
|
print("")
|
||||||
sys.exit(1)
|
sys.exit(1)
|
||||||
|
|
||||||
|
class LoggingCompleted(Exception):
|
||||||
|
pass
|
||||||
|
|
||||||
class MavlinkLogStreaming():
|
class MavlinkLogStreaming():
|
||||||
'''Streams log data via MAVLink.
|
'''Streams log data via MAVLink.
|
||||||
|
@ -49,6 +52,7 @@ class MavlinkLogStreaming():
|
||||||
self.logging_started = False
|
self.logging_started = False
|
||||||
self.num_dropouts = 0
|
self.num_dropouts = 0
|
||||||
self.target_component = 1
|
self.target_component = 1
|
||||||
|
self.got_sig_int = False
|
||||||
|
|
||||||
def debug(self, s, level=1):
|
def debug(self, s, level=1):
|
||||||
'''write some debug text'''
|
'''write some debug text'''
|
||||||
|
@ -67,13 +71,24 @@ class MavlinkLogStreaming():
|
||||||
mavutil.mavlink.MAV_CMD_LOGGING_STOP, 0,
|
mavutil.mavlink.MAV_CMD_LOGGING_STOP, 0,
|
||||||
0, 0, 0, 0, 0, 0, 0)
|
0, 0, 0, 0, 0, 0, 0)
|
||||||
|
|
||||||
|
def _int_handler(self, sig, frame):
|
||||||
|
self.got_sig_int = True
|
||||||
|
|
||||||
def read_messages(self):
|
def read_messages(self):
|
||||||
''' main loop reading messages '''
|
''' main loop reading messages '''
|
||||||
measure_time_start = timer()
|
measure_time_start = timer()
|
||||||
measured_data = 0
|
measured_data = 0
|
||||||
|
|
||||||
next_heartbeat_time = timer()
|
next_heartbeat_time = timer()
|
||||||
|
old_handler = signal.signal(signal.SIGINT, self._int_handler)
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
|
if self.got_sig_int:
|
||||||
|
signal.signal(signal.SIGINT, old_handler)
|
||||||
|
self.got_sig_int = False
|
||||||
|
print('\nStopping log...')
|
||||||
|
self.stop_log()
|
||||||
|
# Continue reading until we get an ACK
|
||||||
|
|
||||||
# handle heartbeat sending
|
# handle heartbeat sending
|
||||||
heartbeat_time = timer()
|
heartbeat_time = timer()
|
||||||
|
@ -120,6 +135,9 @@ class MavlinkLogStreaming():
|
||||||
print('Logging started. Waiting for Header...')
|
print('Logging started. Waiting for Header...')
|
||||||
else:
|
else:
|
||||||
raise Exception('Logging start failed', m.result)
|
raise Exception('Logging start failed', m.result)
|
||||||
|
elif m.command == mavutil.mavlink.MAV_CMD_LOGGING_STOP and \
|
||||||
|
m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED:
|
||||||
|
raise LoggingCompleted()
|
||||||
return None, 0, 0
|
return None, 0, 0
|
||||||
|
|
||||||
# m is either 'LOGGING_DATA_ACKED' or 'LOGGING_DATA':
|
# m is either 'LOGGING_DATA_ACKED' or 'LOGGING_DATA':
|
||||||
|
@ -137,7 +155,8 @@ class MavlinkLogStreaming():
|
||||||
|
|
||||||
if m.get_type() == 'LOGGING_DATA':
|
if m.get_type() == 'LOGGING_DATA':
|
||||||
if not self.got_header_section:
|
if not self.got_header_section:
|
||||||
print('Header received in {:0.2f}s'.format(timer()-self.start_time))
|
print('Header received in {:0.2f}s (size: {:.1f} KB)'.format(
|
||||||
|
timer()-self.start_time, self.file.tell()/1024))
|
||||||
self.logging_started = True
|
self.logging_started = True
|
||||||
self.got_header_section = True
|
self.got_header_section = True
|
||||||
self.last_sequence = m.sequence
|
self.last_sequence = m.sequence
|
||||||
|
@ -256,14 +275,10 @@ def main():
|
||||||
print('Starting log...')
|
print('Starting log...')
|
||||||
mav_log_streaming.start_log()
|
mav_log_streaming.start_log()
|
||||||
mav_log_streaming.read_messages()
|
mav_log_streaming.read_messages()
|
||||||
|
|
||||||
print('Stopping log')
|
|
||||||
mav_log_streaming.stop_log()
|
|
||||||
|
|
||||||
except KeyboardInterrupt:
|
except KeyboardInterrupt:
|
||||||
print('Stopping log')
|
print('Aborting')
|
||||||
mav_log_streaming.stop_log()
|
except LoggingCompleted:
|
||||||
|
print('Done')
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
main()
|
main()
|
||||||
|
|
Loading…
Reference in New Issue