Reimpemented support for 16488

parsers/sensors/adis_parser.py

@@ -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: