Merge branch 'develop' of gitlab.kyb.guru:enfo/sentiboard-utils into develop (88e76fad) · Commits · senti / sentiboard-utils

converters/sb2matlab.py

+8 −8

Original line number	Diff line number	Diff line
		@@ -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

+16 −28

Original line number	Diff line number	Diff line
		@@ -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

+26 −26

Original line number	Diff line number	Diff line
		@@ -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

+3 −3

Original line number	Diff line number	Diff line
		@@ -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

+1 −0

Original line number	Diff line number	Diff line
		@@ -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}"