Commit bfc862f4 authored by enfo's avatar enfo

Reimpemented support for 16488

parent d7c27900
Pipeline #129 passed with stages
in 1 minute and 2 seconds
......@@ -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:
......
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