mirror of
https://github.com/okalachev/flix.git
synced 2026-01-11 21:46:55 +00:00
Refactor Wi-Fi log download
Use MAVLink LOG_REQUEST_DATA and LOG_DATA for download log instead of console. Make Wi-Fi download default way of downloading the log. Make `log` command only print the header and `log dump` dump the log.
This commit is contained in:
42
tools/log.py
42
tools/log.py
@@ -3,21 +3,49 @@
|
||||
# Download flight log remotely and save to file
|
||||
|
||||
import os
|
||||
import time
|
||||
import datetime
|
||||
import struct
|
||||
from pymavlink.dialects.v20.common import MAVLink_log_data_message
|
||||
from pyflix import Flix
|
||||
|
||||
DIR = os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
flix = Flix()
|
||||
|
||||
print('Downloading log...')
|
||||
lines = flix.cli('log').splitlines()
|
||||
|
||||
# sort by timestamp
|
||||
header = lines.pop(0)
|
||||
lines.sort(key=lambda line: float(line.split(',')[0]))
|
||||
header = flix.cli('log')
|
||||
print('Received header:\n- ' + '\n- '.join(header.split(',')))
|
||||
|
||||
records = []
|
||||
|
||||
def on_record(msg: MAVLink_log_data_message):
|
||||
global stop
|
||||
stop = time.time() + 1 # extend timeout
|
||||
records.append([])
|
||||
i = 0
|
||||
data = bytes(msg.data)
|
||||
while i + 4 <= msg.count:
|
||||
records[-1].append(struct.unpack('<f', data[i:i+4])[0])
|
||||
i += 4
|
||||
|
||||
stop = time.time() + 3
|
||||
flix.on('mavlink.LOG_DATA', on_record)
|
||||
flix.mavlink.log_request_data_send(flix.system_id, 0, 0, 0, 0xFFFFFFFF)
|
||||
|
||||
while time.time() < stop:
|
||||
time.sleep(1)
|
||||
|
||||
flix.off(on_record)
|
||||
|
||||
records.sort(key=lambda record: record[0])
|
||||
records = [record for record in records if record[0] != 0]
|
||||
|
||||
print(f'Received records: {len(records)}')
|
||||
|
||||
log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
|
||||
content = header.encode() + b'\n' + b'\n'.join(line.encode() for line in lines)
|
||||
log.write(content)
|
||||
log.write(header.encode() + b'\n')
|
||||
for record in records:
|
||||
line = ','.join(f'{value}' for value in record)
|
||||
log.write(line.encode() + b'\n')
|
||||
print(f'Written {os.path.relpath(log.name, os.curdir)}')
|
||||
|
||||
Reference in New Issue
Block a user