Logging works, but nan values choke it up

cflib_groundstation/LogfileHandler.py

@@ -18,6 +18,7 @@ from cflib.crazyflie.log import LogConfig
 import numpy as np
 
 
+
 class LogfileHandler:
     """
     Handles all interactions with cflib.
@@ -45,6 +46,7 @@ class LogfileHandler:
         self.data_log_name = ""
         self.header_id = -1
         self.header = []
+        self.logging_queue = Queue()
 
         self.create_log_data_file()
 
@@ -86,7 +88,7 @@ class LogfileHandler:
         file.close()
         return filename
 
-    def read_all_active_blocks(self):
+    def read_all_active_blocks(self, scf):
         active_blocks = []
         logBlockFile = open("/home/bitcraze/Desktop/groundstation/crazyflie_groundstation/loggingBlocks.txt", "r")
         line = "BEGIN"
@@ -116,7 +118,11 @@ class LogfileHandler:
                 config = LogConfig(data[1], data[2])
                 for i in range(3, len(data)):
                     config.add_variable(data[i], 'float')
+                config.data_received_cb.add_callback(
+                self.logging_callback)
+                scf.cf.log.add_config(config)
                 active_blocks.append(config)
+                
         self.add_config_headers(active_blocks)
         return active_blocks
         
@@ -154,13 +160,33 @@ class LogfileHandler:
     def write_data_points(self, data):
         self.data_log = open(self.data_log_name, 'a')
         line = str(data[0]['timestamp']) + "\t"
+        reorganize = [ 'nan' ] * 12
         for point in data:
-                line += str('%.4f'%(point['data'])) + "\t"
+            try:
+                reorganize[self.header.index(point['signal'])] = point['data']
+            except:
+                print('No header for ' + point['signal'])
+
+
+        for flt in reorganize:
+            if flt == 'nan':
+                line += 'nan\t'
+            else:
+                line += str('%.4f'%(flt)) + "\t"
         line += "\t\r"
         print(line)
         self.data_log.write(line)
         self.data_log.close()
 
+    def logging_callback(self, _timestamp, data, _logconf):
+        """ Whenever data comes in from the logging, it is sent here,
+        which routes it into our specific format for the logging queue. """
+
+        timestamp1 = time.time() - self.start_time
+        for key in data.keys():
+            value_pair = {'timestamp': timestamp1, 'data': data[key],
+                          'signal': key}
+            self.logging_queue.put(value_pair)
                     
 
 

cflib_groundstation/crazyflie_connection.py

@@ -54,9 +54,8 @@ class CrazyflieConnection:
         self.stop_thread = False
         self.setpoint_handler = SetpointHandler()
         self.logfile_handler = LogfileHandler()
-        self.logging_configs = self.logfile_handler.read_all_active_blocks()
+        
         self.timestamp = 0
-        self.logging_queue = Queue()
 
         # self.timer = QTimer()
         # self.timer.timeout.connect(self.update_plot)
@@ -80,6 +79,7 @@ class CrazyflieConnection:
         # sets commander
         self.scf.cf.commander = uCartCommander.Commander(self.scf.cf)
 
+        self.logging_configs = self.logfile_handler.read_all_active_blocks(self.scf)
 
         # connect the crazyflie commander to the setpoint handler
         # refresh the logging page so that it displays the toc
@@ -326,17 +326,9 @@ class CrazyflieConnection:
             self.delete_log_blocks()
         elif id == 1:
             self.delete_log_blocks()
-            self.logging_configs = self.logfile_handler.read_all_active_blocks()
-            for config in self.logging_configs:
-                config.data_received_cb.add_callback(
-                self.logging_callback)
-                self.scf.cf.log.add_config(config)
+            self.logging_configs = self.logfile_handler.read_all_active_blocks(self.scf)
         elif id == 2:
-            self.logging_configs = self.logfile_handler.read_all_active_blocks()
-            for config in self.logging_configs:
-                config.data_received_cb.add_callback(
-                self.logging_callback)
-                self.scf.cf.log.add_config(config)
+            self.logging_configs = self.logfile_handler.read_all_active_blocks(self.scf)
         elif id == 3:
             block_id = command['data'][1]
             self.logging_configs.remove(self.logging_configs[block_id])
@@ -384,23 +376,13 @@ class CrazyflieConnection:
 
     def continous_log(self):
         while not self.stop_thread:
-            if self.logging_queue.qsize() > 3:
+            if self.logfile_handler.logging_queue.qsize() > 3:
                 data = []
                 for i in range(0, 12):
-                    point = self.logging_queue.get()
-                    print(point)
+                    point = self.logfile_handler.logging_queue.get()
                     data.append(point)
                 self.logfile_handler.write_data_points(data)
-                sleep(0.3)
 
 
-    def logging_callback(self, _timestamp, data, _logconf):
-        """ Whenever data comes in from the logging, it is sent here,
-        which routes it into our specific format for the logging queue. """
-
-        timestamp1 = time.time() - self.start_time
-        for key in data.keys():
-            value_pair = {'timestamp': timestamp1, 'data': data[key],
-                          'signal': key}
-            self.logging_queue.put(value_pair)
+    
 

cflib_groundstation/logs/cflie1_2023_11_13_18:14:15.txt

-#Crazyflie#Controller:Unknown
\ No newline at end of file

cflib_groundstation/logs/cflie1_2023_11_13_18:17:50.txt

-#Crazyflie
#Controller:Unknown
#0	time	stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	ctrlStdnt.r_roll	ctrlStdnt.r_pitch	ctrlStdnt.r_yaw	ctrlStdnt.rollRate	ctrlStdnt.pitchRate	ctrlStdnt.yawRate	ctrlStdnt.roll	ctrlStdnt.pitch	ctrlStdnt.yaw
\ No newline at end of file

cflib_groundstation/logs/cflie1_2023_11_13_18:43:23.txt

-#Crazyflie
#Controller:Unknown
#0	time	stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	ctrlStdnt.r_roll	ctrlStdnt.r_pitch	ctrlStdnt.r_yaw	ctrlStdnt.rollRate	ctrlStdnt.pitchRate	ctrlStdnt.yawRate	ctrlStdnt.roll	ctrlStdnt.pitch	ctrlStdnt.yaw
\ No newline at end of file

cflib_groundstation/logs/cflie1_2023_11_13_18:45:48.txt

-#Crazyflie
#Controller:Unknown
#0	time	stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	ctrlStdnt.r_roll	ctrlStdnt.r_pitch	ctrlStdnt.r_yaw	ctrlStdnt.rollRate	ctrlStdnt.pitchRate	ctrlStdnt.yawRate	ctrlStdnt.roll	ctrlStdnt.pitch	ctrlStdnt.yawstateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	
\ No newline at end of file

cflib_groundstation/logs/cflie1_2023_11_13_18:51:23.txt

-#Crazyflie
#Controller:Unknown
\ No newline at end of file

cflib_groundstation/logs/cflie1_2023_11_13_18:51:55.txt

-#Crazyflie
#Controller:Unknown
#0	time	stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	ctrlStdnt.r_roll	ctrlStdnt.r_pitch	ctrlStdnt.r_yaw	ctrlStdnt.rollRate	ctrlStdnt.pitchRate	ctrlStdnt.yawRate	ctrlStdnt.roll	ctrlStdnt.pitch	ctrlStdnt.yaw
\ No newline at end of file

cflib_groundstation/logs/cflie1_2023_11_13_18:52:26.txt

-#Crazyflie
#Controller:Unknown
#0	time	stateEstimate.roll	stateEstimate.pitch	stateEstimate.yaw	ctrlStdnt.r_roll	ctrlStdnt.r_pitch	ctrlStdnt.r_yaw	ctrlStdnt.rollRate	ctrlStdnt.pitchRate	ctrlStdnt.yawRate	ctrlStdnt.roll	ctrlStdnt.pitch	ctrlStdnt.yaw2664.527 -0.2249341905117035	0.1795833259820938	-20.655818939208984	0.0630495622754097	-0.019611896947026253	-0.021850405260920525	0.0	0.0	0.0	0.0	0.0	-20.662267684936523	
2665.559 -0.22593162953853607	0.17787422239780426	-20.668827056884766	-0.003237646771594882	-0.06716358661651611	-0.1893502026796341	0.0	0.0	0.0	0.0	0.0	-20.665260314941406	
2666.608 -0.232166588306427	0.1993357241153717	-20.671485900878906	-0.15982186794281006	-0.14582476019859314	0.21151892840862274	0.0	0.0	0.0	0.0	0.0	-20.670024871826172	
2667.666 -0.23002538084983826	0.20136956870555878	-20.67282485961914	-0.1109040230512619	0.16435162723064423	0.19461053609848022	0.0	0.0	0.0	0.0	0.0	-20.678686141967773	
2668.717 -0.24088990688323975	0.1908612698316574	-20.68611717224121	-0.05335886403918266	0.25089824199676514	0.03748396039009094	0.0	0.0	0.0	0.0	0.0	-20.681615829467773	
2669.788 -0.2471611648797989	0.18524512648582458	-20.680728912353516	0.029240721836686134	0.07677371054887772	0.14828798174858093	0.0	0.0	0.0	0.0	0.0	-20.68077278137207	
2670.836 -0.24013113975524902	0.1678890585899353	-20.677570343017578	0.0710749700665474	0.10382790118455887	0.08601564168930054	0.0	0.0	0.0	0.0	0.0	-20.67952537536621	
2671.903 -0.23463235795497894	0.1790095418691635	-20.68352508544922	0.037242621183395386	0.038598258048295975	-0.171991229057312	0.0	0.0	0.0	0.0	0.0	-20.701509475708008	
2672.956 -0.2289217859506607	0.19495955109596252	-20.708011627197266	-0.21793454885482788	0.0018176068551838398	-0.140023872256279	0.0	0.0	0.0	0.0	0.0	-20.71295738220215	
2674.007 -0.2356119155883789	0.18666185438632965	-20.723102569580078	-0.04160384088754654	0.5091188549995422	-0.2663525938987732	0.0	0.0	0.0	0.0	0.0	-20.731910705566406	
2675.079 -0.23103639483451843	0.18645061552524567	-20.73302459716797	-0.04817742854356766	0.003122511552646756	-0.03841472044587135	0.0	0.0	0.0	0.0	0.0	-20.731088638305664	
2676.137 -0.21975839138031006	0.1698170006275177	-20.736764907836914	0.009844992309808731	0.1422719806432724	0.10667766630649567	0.0	0.0	0.0	0.0	0.0	-20.744792938232422	
\ No newline at end of file

cflib_groundstation/logs/cflie1_2023_11_15_22:18:22.txt

-#Crazyflie
#Controller:Unknown
\ No newline at end of file