Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

DFReader: change to python3 only #1003

Merged
merged 4 commits into from
Jan 29, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion CSVReader.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python3
'''
CSV log file reader

Expand Down
171 changes: 72 additions & 99 deletions DFReader.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python3
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So this doesn't reeally accomplish much, as DFReader is usually called from mavlogdump.py via mavutil.py.

We should probably change mavlogdump.py's shebang line as part of this PR.

'''
APM DataFlash log file reader

Expand All @@ -23,7 +23,11 @@
import gzip
import io

from . import mavutil
try:
from . import mavutil
except Exception:
# allows running uninstalled
from pymavlink import mavutil

try:
long # Python 2 has long
Expand Down Expand Up @@ -64,18 +68,12 @@
1.0e-9: "n" # nano
}

def u_ord(c):
return ord(c) if sys.version_info.major < 3 else c

def is_quiet_nan(val):
'''determine if the argument is a quiet nan'''
# Is this a float, and some sort of nan?
if isinstance(val, float) and math.isnan(val):
# quiet nans have more non-zero values:
if sys.version_info.major >= 3:
noisy_nan = bytearray([0x7f, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])
else:
noisy_nan = "\x7f\xf8\x00\x00\x00\x00\x00\x00"
noisy_nan = bytearray([0x7f, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])
return struct.pack(">d", val) != noisy_nan
else:
return False
Expand All @@ -98,7 +96,7 @@ def __init__(self, type, name, flen, format, columns, oldfmt=None):
msg_types = []
msg_fmts = []
for c in format:
if u_ord(c) == 0:
if c == 0:
break
try:
msg_fmts.append(c)
Expand Down Expand Up @@ -195,9 +193,6 @@ def to_string(s):
'''desperate attempt to convert a string regardless of what garbage we get'''
if isinstance(s, str):
return s
if sys.version_info[0] == 2:
# In python2 we want to return unicode for passed in unicode
return s
return s.decode(errors="backslashreplace")

def null_term(string):
Expand Down Expand Up @@ -274,21 +269,14 @@ def get_type(self):
return self.fmt.name

def __str__(self):
is_py3 = sys.version_info >= (3,0)
ret = "%s {" % self.fmt.name
col_count = 0
for c in self.fmt.columns:
val = self.__getattr__(c)
if is_quiet_nan(val):
val = "qnan"
# Add the value to the return string
if is_py3:
ret += "%s : %s, " % (c, val)
else:
try:
ret += "%s : %s, " % (c, val)
except UnicodeDecodeError:
ret += "%s : %s, " % (c, to_string(val))
ret += "%s : %s, " % (c, val)
col_count += 1
if col_count != 0:
ret = ret[:-2]
Expand Down Expand Up @@ -353,7 +341,6 @@ def dump_verbose_bitmask(self, f, c, val, field_metadata):
pass

def dump_verbose(self, f):
is_py3 = sys.version_info >= (3,0)
timestamp = "%s.%03u" % (
time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(self._timestamp)),
int(self._timestamp*1000.0)%1000)
Expand All @@ -376,13 +363,7 @@ def dump_verbose(self, f):
if is_quiet_nan(val):
val = "qnan"
# Output the field label and value
if is_py3:
f.write(" %s: %s" % (c, val))
else:
try:
f.write(" %s: %s" % (c, val))
except UnicodeDecodeError:
f.write(" %s: %s" % (c, to_string(val)))
f.write(" %s: %s" % (c, val))

# If this is a bitmask, then append the hex value
if c in field_metadata_by_name:
Expand Down Expand Up @@ -422,7 +403,6 @@ def dump_verbose(self, f):
def get_msgbuf(self):
'''create a binary message buffer for a message'''
values = []
is_py2 = sys.version_info < (3,0)
for i in range(len(self.fmt.columns)):
if i >= len(self.fmt.msg_mults):
continue
Expand All @@ -431,19 +411,13 @@ def get_msgbuf(self):
if name == 'Mode' and 'ModeNum' in self.fmt.columns:
name = 'ModeNum'
v = self.__getattr__(name)
if is_py2:
if isinstance(v,unicode): # NOQA
v = str(v)
elif isinstance(v, array.array):
v = v.tostring()
else:
if isinstance(v,str):
try:
v = bytes(v,'ascii')
except UnicodeEncodeError:
v = v.encode()
elif isinstance(v, array.array):
v = v.tobytes()
if isinstance(v,str):
try:
v = bytes(v,'ascii')
except UnicodeEncodeError:
v = v.encode()
elif isinstance(v, array.array):
v = v.tobytes()
if mul is not None:
v /= mul
v = int(round(v))
Expand Down Expand Up @@ -677,12 +651,8 @@ def dot_pymavlink(*args):
@staticmethod
def download_url(url):
'''download a URL and return the content'''
if sys.version_info.major < 3:
from urllib2 import urlopen as url_open
from urllib2 import URLError as url_error
else:
from urllib.request import urlopen as url_open
from urllib.error import URLError as url_error
from urllib.request import urlopen as url_open
from urllib.error import URLError as url_error
try:
resp = url_open(url)
except url_error as e:
Expand Down Expand Up @@ -1079,7 +1049,7 @@ class DFReader_binary(DFReader):
def __init__(self, filename, zero_time_base=False, progress_callback=None):
DFReader.__init__(self)
# read the whole file into memory for simplicity
self.filehandle = open(filename, 'r')
self.filehandle = open(filename, 'rb')
self.filehandle.seek(0, 2)
self.data_len = self.filehandle.tell()
self.filehandle.seek(0)
Expand All @@ -1091,9 +1061,6 @@ def __init__(self, filename, zero_time_base=False, progress_callback=None):
self.HEAD1 = 0xA3
self.HEAD2 = 0x95
self.unpackers = {}
if sys.version_info.major < 3:
self.HEAD1 = chr(self.HEAD1)
self.HEAD2 = chr(self.HEAD2)
self.formats = {
0x80: DFFormat(0x80,
'FMT',
Expand Down Expand Up @@ -1122,15 +1089,16 @@ def rewind(self):

def init_arrays(self, progress_callback=None):
'''initialise arrays for fast recv_match()'''
self.offsets = []
self.counts = []
offsets = []
counts = []
formats = self.formats
Comment on lines +1092 to +1094
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This would not seem to be related to the PR title?

I mean, probably not a bad change, but...

self._count = 0
self.name_to_id = {}
self.id_to_name = {}
type_instances = {}
for i in range(256):
self.offsets.append([])
self.counts.append(0)
offsets.append([])
counts.append(0)
fmt_type = 0x80
fmtu_type = None
unit_type = None
Expand All @@ -1140,34 +1108,35 @@ def init_arrays(self, progress_callback=None):
HEAD1 = self.HEAD1
HEAD2 = self.HEAD2
lengths = [-1] * 256
data_len = self.data_len
data_map = self.data_map

while ofs+3 < self.data_len:
hdr = self.data_map[ofs:ofs+3]
if hdr[0] != HEAD1 or hdr[1] != HEAD2:
while ofs+3 < data_len:
h1, h2, mtype = data_map[ofs:ofs+3]
if h1 != HEAD1 or h2 != HEAD2:
# avoid end of file garbage, 528 bytes has been use consistently throughout this implementation
# but it needs to be at least 249 bytes which is the block based logging page size (256) less a 6 byte header and
# one byte of data. Block based logs are sized in pages which means they can have up to 249 bytes of trailing space.
if self.data_len - ofs >= 528 or self.data_len < 528:
print("bad header 0x%02x 0x%02x at %d" % (u_ord(hdr[0]), u_ord(hdr[1]), ofs), file=sys.stderr)
if data_len - ofs >= 528 or data_len < 528:
print("bad header 0x%02x 0x%02x at %d" % (h1, h2, ofs), file=sys.stderr)
ofs += 1
continue
mtype = u_ord(hdr[2])
self.offsets[mtype].append(ofs)
offsets[mtype].append(ofs)

if lengths[mtype] == -1:
if not mtype in self.formats:
if self.data_len - ofs >= 528 or self.data_len < 528:
if not mtype in formats:
if data_len - ofs >= 528 or data_len < 528:
print("unknown msg type 0x%02x (%u) at %d" % (mtype, mtype, ofs),
file=sys.stderr)
break
self.offset = ofs
self._parse_next()
fmt = self.formats[mtype]
fmt = formats[mtype]
lengths[mtype] = fmt.len
elif self.formats[mtype].instance_field is not None:
fmt = self.formats[mtype]
elif formats[mtype].instance_field is not None:
fmt = formats[mtype]
# see if we've has this instance value before
idata = self.data_map[ofs+3+fmt.instance_ofs:ofs+3+fmt.instance_ofs+fmt.instance_len]
idata = data_map[ofs+3+fmt.instance_ofs:ofs+3+fmt.instance_ofs+fmt.instance_len]
if not mtype in type_instances:
type_instances[mtype] = set()
if not idata in type_instances[mtype]:
Expand All @@ -1176,22 +1145,22 @@ def init_arrays(self, progress_callback=None):
self.offset = ofs
self._parse_next()

self.counts[mtype] += 1
counts[mtype] += 1
mlen = lengths[mtype]

if mtype == fmt_type:
body = self.data_map[ofs+3:ofs+mlen]
body = data_map[ofs+3:ofs+mlen]
if len(body)+3 < mlen:
break
fmt = self.formats[mtype]
fmt = formats[mtype]
elements = list(struct.unpack(fmt.msg_struct, body))
ftype = elements[0]
mfmt = DFFormat(
ftype,
null_term(elements[2]), elements[1],
null_term(elements[3]), null_term(elements[4]),
oldfmt=self.formats.get(ftype,None))
self.formats[ftype] = mfmt
oldfmt=formats.get(ftype,None))
formats[ftype] = mfmt
self.name_to_id[mfmt.name] = mfmt.type
self.id_to_name[mfmt.type] = mfmt.name
if mfmt.name == 'FMTU':
Expand All @@ -1203,33 +1172,33 @@ def init_arrays(self, progress_callback=None):

# Handle FMTU messages by updating the DFFormat class with the
# unit/multiplier information
if fmtu_type is not None and mtype == fmtu_type:
fmt = self.formats[mtype]
body = self.data_map[ofs+3:ofs+mlen]
if mtype == fmtu_type:
fmt = formats[mtype]
body = data_map[ofs+3:ofs+mlen]
if len(body)+3 < mlen:
break
elements = list(struct.unpack(fmt.msg_struct, body))
ftype = int(elements[1])
if ftype in self.formats:
fmt2 = self.formats[ftype]
if ftype in formats:
fmt2 = formats[ftype]
if 'UnitIds' in fmt.colhash:
fmt2.set_unit_ids(null_term(elements[fmt.colhash['UnitIds']]), self.unit_lookup)
if 'MultIds' in fmt.colhash:
fmt2.set_mult_ids(null_term(elements[fmt.colhash['MultIds']]), self.mult_lookup)

# Handle UNIT messages by updating the unit_lookup dictionary
if unit_type is not None and mtype == unit_type:
fmt = self.formats[mtype]
body = self.data_map[ofs+3:ofs+mlen]
if mtype == unit_type:
fmt = formats[mtype]
body = data_map[ofs+3:ofs+mlen]
if len(body)+3 < mlen:
break
elements = list(struct.unpack(fmt.msg_struct, body))
self.unit_lookup[chr(elements[1])] = null_term(elements[2])

# Handle MULT messages by updating the mult_lookup dictionary
if mult_type is not None and mtype == mult_type:
fmt = self.formats[mtype]
body = self.data_map[ofs+3:ofs+mlen]
if mtype == mult_type:
fmt = formats[mtype]
body = data_map[ofs+3:ofs+mlen]
if len(body)+3 < mlen:
break
elements = list(struct.unpack(fmt.msg_struct, body))
Expand All @@ -1243,13 +1212,16 @@ def init_arrays(self, progress_callback=None):

ofs += mlen
if progress_callback is not None:
new_pct = (100 * ofs) // self.data_len
new_pct = (100 * ofs) // data_len
if new_pct != pct:
progress_callback(new_pct)
pct = new_pct

self.offsets = offsets
self.counts = counts
self.formats = formats
for i in range(256):
self._count += self.counts[i]
self._count += counts[i]
self.offset = 0

def last_timestamp(self):
Expand Down Expand Up @@ -1328,7 +1300,7 @@ def _parse_next(self):
file=sys.stderr)
skip_type = None
# check we recognise this message type:
msg_type = u_ord(hdr[2])
msg_type = hdr[2]
if msg_type in self.formats:
# recognised message found
self.prev_type = msg_type
Expand All @@ -1338,7 +1310,7 @@ def _parse_next(self):
# are easily recognisable in the "Skipped bytes"
# message.
if skip_type is None:
skip_type = (u_ord(hdr[0]), u_ord(hdr[1]), u_ord(hdr[2]))
skip_type = (hdr[0], hdr[1], hdr[2])
skip_start = self.offset
self.offset += 1
self.remaining -= 1
Expand Down Expand Up @@ -1584,8 +1556,7 @@ def _parse_next(self):
if endline < self.offset:
break
s = self.data_map[self.offset:endline].rstrip()
if sys.version_info.major >= 3:
s = s.decode('utf-8')
s = s.decode('utf-8')
elements = s.split(self.delimiter)
self.offset = endline+1
if len(elements) >= 2:
Expand Down Expand Up @@ -1684,6 +1655,7 @@ def last_timestamp(self):

if __name__ == "__main__":
use_profiler = False
parse_messages = False
if use_profiler:
from line_profiler import LineProfiler
profiler = LineProfiler()
Expand All @@ -1699,11 +1671,12 @@ def last_timestamp(self):
log = DFReader_binary(filename)
#bfile = filename + ".bin"
#bout = open(bfile, 'wb')
while True:
m = log.recv_msg()
if m is None:
break
#bout.write(m.get_msgbuf())
#print(m)
if parse_messages:
while True:
m = log.recv_msg()
if m is None:
break
#bout.write(m.get_msgbuf())
#print(m)
if use_profiler:
profiler.print_stats()
profiler.print_stats()
Loading