Reimpemented support for 16488 (bfc862f4) · Commits · senti / sentiboard-utils

parsers/sensors/adis_parser.py

+14 −24

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))

		@@ -192,6 +181,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:
		@@ -208,7 +198,7 @@ if __name__ == '__main__':
		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('% 11d % 11d % 11d' % (package.tov, package.tot, package.toa))
		stdout.flush()
		i += 1
		finally: