Acceleration parsing for raw blackbox

Author: ElvinC

blackbox_extract.py

@@ -14,7 +14,9 @@ def __init__(self, path):
         self.n_of_logs = self.parser.reader.log_count
 
         self.gyro_scale = self.parser.headers["gyro_scale"] #should be already scaled in the fc
+        self.acc_scale = 1/self.parser.headers["acc_1G"]
         self.final_gyro_data = []
+        self.final_acc_data = []
         self.extracted = False
         self.camera_angle = None
 
@@ -76,9 +78,9 @@ def get_gyro_data(self,cam_angle_degrees=0):
 
         return self.final_gyro_data
 
-    def get_untransformed_gyro_data(self):
+    def get_untransformed_imu_data(self):
         if self.extracted:
-            return np.array(self.final_gyro_data)
+            return np.array(self.final_gyro_data), np.array(self.final_acc_data)
 
         r  = Rotation.from_euler('x', self.camera_angle, degrees=True)
 
@@ -89,32 +91,52 @@ def get_untransformed_gyro_data(self):
             gy = self.parser.field_names.index('gyroADC[2]')
             gz = self.parser.field_names.index('gyroADC[0]')
             data_frames = []
+
+            max_index = gy
+
+            acc_index = None
+            if "accSmooth[0]" in self.parser.field_names:
+                acc_index = self.parser.field_names.index("accSmooth[0]")
+                max_index = max(acc_index + 2, gy)
 
+            acc_frames = []
+
             last_t = 0
             for frame in self.parser.frames():
-                #print(len(frame.data))
-                if len(frame.data) > gy and ((0 < (frame.data[t] - last_t) < self.max_data_gab) or (last_t == 0)):
+                if len(frame.data) > max_index and ((0 < (frame.data[t] - last_t) < 1000000 * self.max_data_gab) or (last_t == 0)):
 
                     data_frames.append([frame.data[t], frame.data[gx], frame.data[gy], frame.data[gz]])
+                    
+                    if acc_index:
+                        acc_frames.append([frame.data[t], frame.data[acc_index+1], frame.data[acc_index+2], frame.data[acc_index]])
+
                     last_t = frame.data[t]
 
-            if len(data_frames) < 2:
-                return False
             self.final_gyro_data.extend(data_frames)
+            if acc_index:
+                self.final_acc_data.extend(acc_frames)
 
 
         self.final_gyro_data = np.array(self.final_gyro_data, dtype=np.float64)
+        #if self.final_gyro_data.shape[0] < 2:
+        #    print("No valid motion data")
+        #    return False, False
         self.final_gyro_data[:,0] /= 1000000
         self.final_gyro_data[:,1:] *= np.pi/180
 
+        if acc_index:
+            self.final_acc_data = np.array(self.final_acc_data, dtype=np.float64)
+            self.final_acc_data[:,0] /= 1000000
+            self.final_acc_data[:,1:] *= self.acc_scale
+
         # rough gyro rate assumed to be constant
 
         self.gyro_rate = self.final_gyro_data.shape[0]/(self.final_gyro_data[-1,0] - self.final_gyro_data[0,0])
 
 
         self.extracted = True
 
-        return self.final_gyro_data
+        return self.final_gyro_data, self.final_acc_data
 
 
 

gyro_integrator.py

@@ -137,12 +137,13 @@ def integrate_all(self, use_acc = False):
                 if apply_complementary:
                     if complementary_mask[i]:
                         avec = self.acc[i][1:]
+                        avec /= np.linalg.norm(avec)
 
                         accWorldVec = quat.rotate_vector_fast(self.orientation, avec)
                         correctionWorld = np.cross(accWorldVec, self.grav_vec)
 
                         # high weight for first two seconds to "lock" it, then 
-                        weight = 10 if this_time - start_time < 1.5 else 0.6
+                        weight = 10 if this_time - start_time < 1.5 else 0.5
                         correctionBody = weight * quat.rotate_vector_fast(quat.conjugate(self.orientation), correctionWorld)
                         omega = omega + correctionBody
 

gyrolog.py

@@ -626,10 +626,10 @@ def extract_log_internal(self, filename):
 
         try:
             bbe = BlackboxExtractor(filename)
-            self.gyro = bbe.get_gyro_data(cam_angle_degrees=self.angle_setting)
+            self.gyro, self.acc = bbe.get_untransformed_imu_data()
 
             return True
-        except:
+        except: # TODO: change
             print("Error reading raw blackbox file. Try converting to CSV in blackbox explorer")
             return False
 
@@ -1152,9 +1152,9 @@ def guess_log_type_from_log(logfile, check_data = False):
         #"test_clips/RC_0038_210813211513.MP4",
         #"test_clips/RC_0031_210722220523.MP4",
         #"test_clips/Runcam/RC_0036_filtered.MP4",
-        "test_clips/DJIG0043wiebe.mp4",
-        "test_clips/GX016015.MP4",
-        "test_clips/nivim_insta360.mp4",
+        #"test_clips/DJIG0043wiebe.mp4",
+        #"test_clips/GX016015.MP4",
+        #"test_clips/nivim_insta360.mp4",
         "test_clips/Tiago_Ferreira_5_inch.mp4",
         "test_clips/MasterTim17_caddx.mp4",
         "test_clips/starling2.MOV",