Skip to content

S
sentiboard-utils
Commit 88e76fad authored by enfo's avatar enfo
Browse files
Options

Merge branch 'develop' of gitlab.kyb.guru:enfo/sentiboard-utils into develop

parents 7737986b 8f972c5e
Hide whitespace changes
Inline Side-by-side
Showing with 54 additions and 65 deletions
converters/sb2matlab.py
View file @ 88e76fad
......@@ -40,7 +40,7 @@ def extract_packages(fname, parsers):
try:
pack = parser.parse_buffer(raw_pack)
except Exception as e:
print 'Could not parse package', e
print('Could not parse package: %s' % e)
continue
except EOFError:
break
......@@ -52,7 +52,7 @@ def extract_packages(fname, parsers):
packages[raw_pack.package_type] = data
for k, v in pack.dict.iteritems():
for k, v in pack.dict.items():
try:
data[k].append(v)
except KeyError:
......@@ -62,19 +62,19 @@ def extract_packages(fname, parsers):
def handle_data_directory(sb_data_dir, parsers, outdir):
global data
for i in xrange(1, 101):
for i in range(1, 101):
if i == 100:
raise IndexError('Could not find proper directory')
odir = outdir + '_%02d' % i
if not os.path.exists(odir):
os.mkdir(odir, 0770)
os.mkdir(odir, 0o0770)
break
if os.listdir(odir) == []:
break
outdir = os.path.abspath(odir)
print('Saving to %s' % outdir)
print
print('')
globname = glob(os.path.join(sb_data_dir, '*.*'))
......@@ -103,14 +103,14 @@ def do_combine_data(data_dir, combined_dir, overwrite, newnames=None):
sensor_files = {}
for pardir in sorted(glob(os.path.join(data_dir, '*_[0-9]*'))):
for p in glob(os.path.join(pardir, '*')):
print 'Handling', os.path.abspath(p)
print('Handling %s' % os.path.abspath(p))
_, fname = os.path.split(p)
try:
sensor_files[fname].append(p)
except KeyError:
sensor_files[fname] = [p]
for n, filelist in sensor_files.iteritems():
for n, filelist in sensor_files.items():
try:
newname = newnames[n]
except KeyError:
......@@ -157,4 +157,4 @@ if __name__ == '__main__':
try:
handle_data_directory(sb_data_dir, parsers, outdir)
finally:
print 'Done'
print('Done')
parsers/sensors/adis_parser.py
View file @ 88e76fad
......@@ -78,19 +78,14 @@ class AdisPackage(object):
self.data = [self.tov] + self._message.data + self.deltang + self.deltvel
self.dict = {'tov': self._tov, 'toa': self._toa, 'tot': self.tot,
'timestamp': self.timestamp, 'temp': self.temp}
'timestamp': self.timestamp, 'temp': self.temp,
'deltvel': self.deltvel, 'deltang': self.deltang}
if prod_id == 16488:
self.barometer = barom_scale(barometer)
self.mag = [mag_scale(m) for m in mag]
self.data += self.mag + [self.barometer, self.temp]
self.dict.update({'mag_x': self.mag[0], 'mag_y': self.mag[1], 'mag_z': self.mag[2],
'barometer': self.barometer})
elif prod_id == 16490:
self.dict.update({'deltvel': self.deltvel, 'deltang': self.deltang})
if self.status is not None:
self.dict.update({'status': self.status})
class AdisParser(object):
allowed_ids = [b'\x65\x40', b'\x68\x40', b'\x6A\x40']
......@@ -106,7 +101,7 @@ class AdisParser(object):
if prod_id == 16490:
package = struct.unpack('<h6i3H', parsebuf)
else:
package = struct.unpack('>h12i3hi3Hh', parsebuf)
package = struct.unpack('<h6i3hi3H', parsebuf)
except Exception as e:
print('ERROR: %s (str was len %d)' % (e, len(parsebuf)))
......@@ -124,41 +119,35 @@ class AdisParser(object):
(deltang_x, deltang_y, deltang_z),
(deltvel_x, deltvel_y, deltvel_z),
temp, status=sys_e_flag)
else:
if not MAG_ON:
elif prod_id == 16488:
if MAG_ON:
temp, \
gyro_x, gyro_y, gyro_z, \
accl_x, accl_y, accl_z, \
deltang_x, deltang_y, deltang_z, \
deltvel_x, deltvel_y, deltvel_z, \
mag_x, mag_y, mag_z, \
barom, \
sys_e_flag, diag_sts,\
prod_id2 \
= package
mag_x, mag_y, mag_z, barometer = [0.0]*4
else:
temp, \
gyro_x, gyro_y, gyro_z, \
gyro_x_low, gyro_y_low, gyro_z_low, \
accl_x, accl_y, accl_z, \
accl_x_low, accl_y_low, accl_z_low, \
deltang_x, deltang_y, deltang_z, \
deltvel_x, deltvel_y, deltvel_z, \
mag_x, mag_y, mag_z, \
barom, \
sys_e_flag, diag_sts,\
prod_id2 \
= package
barometer = 0 #[0.0]*1
mag_x, mag_y, mag_z, barom = [0.0]*4
package = AdisPackage(buf.tov, buf.toa, buf.tot, buf.timestamp,
prod_id,
(deltang_x, deltang_y, deltang_z),
(deltvel_x, deltvel_y, deltvel_z),
temp,
(mag_x, mag_y, mag_z),
barometer, temp,
(gyro_x, gyro_y, gyro_z),
(accl_x, accl_y, accl_z))
barom, status=sys_e_flag)
if not (sys_e_flag == 1 or sys_e_flag == 0):
print('\nError: %X %X\n' % (sys_e_flag, diag_sts))
......@@ -170,7 +159,6 @@ if __name__ == '__main__':
import sys
import numpy as np
PATH = '/dev/ttySyncboard02'
PATH = 'COM8'
if len(sys.argv) > 1:
PATH = sys.argv[1]
reader = SyncboardReader(PATH)
......@@ -192,6 +180,7 @@ if __name__ == '__main__':
try:
package = parser.parse_buffer(package_buf)
except TypeError as e:
raise
print('Could not parse package:', e)
continue
if package == None:
......@@ -205,10 +194,9 @@ if __name__ == '__main__':
if timediff == 0:
timediff = 1
fps = float(i)/(timediff)
stdout.write('\rfps: % 4.1f\t temp:% 3.2f\t' % tuple([fps, package.temp]))
stdout.write('accl: % 3.4f\t' % (np.linalg.norm(np.array(package.deltvel))*fps))
stdout.write(str(np.array(package.deltvel)*fps))
stdout.write('% 11d % 11d % 11d' % (package.tov, package.tot, package.toa))
stdout.write('\rfps: % 6.1f | temp: % 5.2f ' % tuple([fps, package.temp]))
stdout.write('| accl: [% 7.3f, % 7.3f, % 7.3f]' % tuple(np.array(package.deltvel)*fps))
stdout.write(' (% 7.3f)' % (np.linalg.norm(np.array(package.deltvel))*fps))
stdout.flush()
i += 1
finally:
......
parsers/sensors/ublox.py
View file @ 88e76fad
......@@ -40,7 +40,7 @@ class UbloxPackage(object):
class UbloxPackageRxmRawX(UbloxPackage):
ubx_name = 'RXM-RAWX'
ubx_id = '\x02\x15'
ubx_id = b'\x02\x15'
def __init__(self, ubx_id, tov, package, toa, tot, timestamp):
self._name = self.ubx_name
UbloxPackage.__init__(self, ubx_id, tov, package, toa, tot, timestamp)
......@@ -82,7 +82,7 @@ class UbloxPackageRxmRawX(UbloxPackage):
class UbloxPackageRxmSfrbX(UbloxPackage):
ubx_name = 'RXM-SFRBX'
ubx_id = '\x02\x13'
ubx_id = b'\x02\x13'
def __init__(self, ubx_id, tov, package, toa, tot, timestamp):
self._name = self.ubx_name
......@@ -107,7 +107,7 @@ class UbloxPackageRxmSfrbX(UbloxPackage):
class UbloxPackageRxmMeasX(UbloxPackage):
ubx_name = 'RXM-MEASX'
ubx_id = '\x02\x14'
ubx_id = b'\x02\x14'
def __init__(self, ubx_id, tov, package, toa, tot, timestamp):
self._name = self.ubx_name
UbloxPackage.__init__(self, ubx_id, tov, package, toa, tot, timestamp)
......@@ -145,7 +145,7 @@ class UbloxPackageRxmMeasX(UbloxPackage):
class UbloxPackageNavPosLLH(UbloxPackage):
ubx_name = 'NAV-POSLLH'
ubx_id = '\x01\x02'
ubx_id = b'\x01\x02'
def __init__(self, ubx_id, tov, package, toa, tot, timestamp):
UbloxPackage.__init__(self, ubx_id, tov, package, toa, tot, timestamp)
......@@ -155,7 +155,7 @@ class UbloxPackageNavPosLLH(UbloxPackage):
class UbloxPackageNavPosECEF(UbloxPackage):
ubx_name = 'NAV-POSECEF'
ubx_id = '\x01\x01'
ubx_id = b'\x01\x01'
def __init__(self, ubx_id, tov, package, toa, tot, timestamp):
UbloxPackage.__init__(self, ubx_id, tov, package, toa, tot, timestamp)
......@@ -165,7 +165,7 @@ class UbloxPackageNavPosECEF(UbloxPackage):
class UbloxPackageNavVelEcef(UbloxPackage):
ubx_name = 'NAV-VELECEF'
ubx_id = '\x01\x11'
ubx_id = b'\x01\x11'
def __init__(self, ubx_id, tov, package, toa, tot, timestamp):
UbloxPackage.__init__(self, ubx_id, tov, package, toa, tot, timestamp)
......@@ -175,7 +175,7 @@ class UbloxPackageNavVelEcef(UbloxPackage):
class UbloxPackageNavVelNED(UbloxPackage):
ubx_name = 'NAV-VELNED'
ubx_id = '\x01\x12'
ubx_id = b'\x01\x12'
def __init__(self, ubx_id, tov, package, toa, tot, timestamp):
UbloxPackage.__init__(self, ubx_id, tov, package, toa, tot, timestamp)
......@@ -186,7 +186,7 @@ class UbloxPackageNavVelNED(UbloxPackage):
class UbloxPackagePVT(UbloxPackage):
ubx_name = 'NAV-PVT'
ubx_id = '\x01\x07'
ubx_id = b'\x01\x07'
def __init__(self, ubx_id, tov, package, toa, tot, timestamp):
UbloxPackage.__init__(self, ubx_id, tov, package, toa, tot, timestamp)
......@@ -209,7 +209,7 @@ class UbloxPackagePVT(UbloxPackage):
class UbloxPackageNavClock(UbloxPackage):
ubx_name = 'NAV-CLOCK'
ubx_id = '\x01\x22'
ubx_id = b'\x01\x22'
def __init__(self, ubx_id, tov, package, toa, tot, timestamp):
UbloxPackage.__init__(self, ubx_id, tov, package, toa, tot, timestamp)
......@@ -225,21 +225,21 @@ MSG_TYPES = [UbloxPackageRxmRawX, UbloxPackageNavPosLLH, UbloxPackageRxmSfrbX,
UBX_MSG_TYPES = dict([(t.ubx_id, t) for t in MSG_TYPES])
UBX_MSG_NAMES = {
'\x01\x32': 'NAV-SBAS',
'\x0B\x31': 'AID-EPH',
'\x0B\x01': 'AID-INI',
'\x02\x14': 'RXM-MEASX',
'\x01\x07': 'NAV-PVT',
'\x01\x12': 'NAV-VELNED',
'\x01\x22': 'NAV-CLOCK'
b'\x01\x32': 'NAV-SBAS',
b'\x0B\x31': 'AID-EPH',
b'\x0B\x01': 'AID-INI',
b'\x02\x14': 'RXM-MEASX',
b'\x01\x07': 'NAV-PVT',
b'\x01\x12': 'NAV-VELNED',
b'\x01\x22': 'NAV-CLOCK'
}
UBX_MSG_NAMES.update([(ubx_id, ubx_type.ubx_name) for ubx_id, ubx_type
in UBX_MSG_TYPES.iteritems()])
in UBX_MSG_TYPES.items()])
class UbloxParser(object):
allowed_ids = ['\xB5\x62']
allowed_ids = [b'\xB5\x62']
multipart = True
def parse_buffer(self, buf):
......@@ -276,9 +276,9 @@ class UbloxParser(object):
return ubx_msg_type(ubx_msg_id, buf.tov, ublox_package, buf.toa,
buf.tot, buf.timestamp)
except Exception, e:
except Exception as e:
raise
print e.message
print(e.message)
return None
if __name__ == '__main__':
......@@ -333,10 +333,10 @@ if __name__ == '__main__':
# sys.stdout.write(text + '\tsplinter:\t%d' % reader.splinter_time)
sys.stdout.flush()
i+=1
except Exception, e:
print
print 'Exception:', e
except Exception as e:
print()
print('Exception: %s' % e)
raise
print
print 'Got a total of %d ublox messages' % i
print ids
print()
print('Got a total of %d ublox messages' % i)
print(ids)
parsers/syncboard_reader.py
View file @ 88e76fad
......@@ -237,9 +237,9 @@ class SyncboardReader(object):
n_bytes, buf = self._sync_package()
if n_bytes == -1:
return None
if buf[:2] == '^C':
if buf[:2] == b'^C':
including_timestamp = True
buf[1] = 'B'
buf[1] = ord('B')
else:
including_timestamp = False
......@@ -324,4 +324,4 @@ if __name__ == '__main__':
FIRST_PACKAGE = READER.get_package()
SECOND_PACKAGE = READER.get_package()
print FIRST_PACKAGE, SECOND_PACKAGE
print(FIRST_PACKAGE, SECOND_PACKAGE)
udev/80-sentiboard.rules
View file @ 88e76fad
......@@ -3,3 +3,4 @@
# This is to be placed in /etc/udev/rules.d/
SUBSYSTEM=="tty", ATTRS{idVendor}=="6666", ATTRS{idProduct}=="4242", SYMLINK+="ttySentiboard%E{ID_USB_INTERFACE_NUM}"
SUBSYSTEM=="tty", ATTRS{idVendor}=="04D8", ATTRS{idProduct}=="EF83", SYMLINK+="ttySentiboard%E{ID_USB_INTERFACE_NUM}"
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment