writing points to file

cflib_groundstation/LogfileHandler.py

 
 from datetime import datetime
 from email.utils import localtime
+from threading import Thread
 from time import time
 from typing import List
 import time
 import struct
+from venv import create
+import os
 
 import cflib.crtp
 from cflib.crazyflie import Crazyflie
@@ -41,7 +44,11 @@ class LogfileHandler:
         self.is_connected = False
         self.param_callback_count = 0
         self.logging_configs = []
-        self.data_log = ""
+        self.data_log = None
+        self.data_log_name = ""
+        self.header_id = 0
+
+        self.create_log_data_file()
 
     
 
@@ -110,15 +117,35 @@ class LogfileHandler:
                     raise Exception("loggingBlocks.txt is not formatted correctly")
                 config = LogConfig(data[1], data[2])
                 for i in range(3, len(data)):
-                    config.add_variable(data[i])
+                    config.add_variable(data[i], 'float')
                 active_blocks.append(config)
+        self.add_config_headers(active_blocks)
         return active_blocks
         
     def create_log_data_file(self):
-        raise Exception
+        self.data_log_name = os.getcwd()
+        self.data_log_name += f"/logs/cflie1_{time.strftime('%Y_%m_%d_%H:%M:%S', time.localtime())}.txt"
+        self.data_log = open(self.data_log_name, 'w')
+        self.data_log.close()
+        self.data_log = open(self.data_log_name, 'a')
+        self.data_log.write("#Crazyflie\r")
+        self.data_log.write("#Controller:Unknown\r")
+        self.data_log.close()
+
+    def add_config_headers(self, config_list: List[LogConfig]):
+        self.data_log = open(self.data_log_name, 'a')
+        header = "#" + str(self.header_id) + "\ttime"
+        for config in config_list:
+            print(config)
+            for variable in config.variables:
+                print("Var: " + variable.name)
+                header += "\t" + variable.name
+        self.data_log.write(header + "\r")
+        self.header_id += 1
+        self.data_log.close()
+
+
 
-    def add_config_headers(self):
-        raise Exception
 
     def start_logging(self):
         raise Exception
@@ -126,8 +153,18 @@ class LogfileHandler:
     def stop_logging(self):
         raise Exception
     
-    def write_data_point(self):
-        raise Exception
+    def write_data_points(self, data, timestamp):
+        self.data_log = open(self.data_log_name, 'a')
+        line = str(timestamp) + " "
+        for group in data:
+            for data in group.values():
+                line += str(data) + "\t"
+        line += "\r"
+        self.data_log.write(line)
+        self.data_log.close()
+
+                    
+
 
 
 

cflib_groundstation/crazyflie_connection.py

@@ -7,7 +7,8 @@ cflib library.
 
 from datetime import datetime
 from email.utils import localtime
-from time import time
+from threading import Thread
+from time import sleep, time
 from typing import List
 import time
 import struct
@@ -45,15 +46,15 @@ class CrazyflieConnection:
         # be in seconds since startup.
         self.start_time = time.time()
 
-        self.logging_queue = Queue()
-
         self.scf = None
         self.is_connected = False
         self.param_callback_count = 0
         self.logging_configs = []
-
+        self.logging_thread = None
+        self.stop_thread = False
         self.setpoint_handler = SetpointHandler()
         self.logfile_handler = LogfileHandler()
+        self.timestamp = 0
 
         # self.timer = QTimer()
         # self.timer.timeout.connect(self.update_plot)
@@ -253,8 +254,16 @@ class CrazyflieConnection:
         id = command['data'][0]
         print(id)
         if id == 0: # logdata?
-            for config in self.logging_configs:
-                self.simple_log(self.scf, config)
+            filename = self.logfile_handler.data_log_name
+            data = bytearray()
+            data += bytes(filename, 'utf-8')
+            responsedata = {
+            "msg_type": (MessageTypeID.RESPLOGFILE_ID),
+            "msg_id": command['msg_id'],
+            "data_len": len(data),
+            "data": data
+            }
+            outputQueue.put(responsedata)
         elif id == 1: # param toc
             params = self.get_param_toc()
             filename = self.logfile_handler.CopyTocToFile(params, True)
@@ -304,24 +313,28 @@ class CrazyflieConnection:
             self.enable_logging()
         elif id == 5:
             self.disable_logging()
+        elif id == 8:
+            self.start_logging()
+        elif id == 9:
+            self.stop_logging()
             
     def simple_log(self, scf, logconf):
         print("Logging...")
         with SyncLogger(scf, logconf) as logger:
             for log_entry in logger:
-                timestamp = log_entry[0]
+                self.timestamp = log_entry[0]
                 data = log_entry[1]
                 logconf_name = log_entry[2]
 
-                print('[%d][%s]: %s' % (timestamp, logconf_name, data))
-
-                break
+                #print('[%d][%s]: %s' % (self.timestamp, logconf_name, data))
+                return data
 
     def enable_logging(self):
         """ Begins logging all configured logging blocks. This is used from
         the controls tab when hitting begin logging. """
         for i in range(0, len(self.logging_configs)):
             self.logging_configs[i].start()
+        
 
     def disable_logging(self):
         """ Stops logging all configured logging blocks. This is used from
@@ -333,6 +346,24 @@ class CrazyflieConnection:
         for block in self.logging_configs:
             block.delete()
             self.logging_configs.remove(block)
+    
+    def start_logging(self):
+        self.stop_thread = False
+        self.logging_thread = Thread(target=self.continous_log)
+        self.logging_thread.start()
+
+    def stop_logging(self):
+        self.stop_thread = True
+
+    def continous_log(self):
+        while not self.stop_thread:
+            data = []
+            for config in self.logging_configs:
+                data.append(self.simple_log(self.scf, config))
+            print(data)
+            self.logfile_handler.write_data_points(data, self.timestamp / 1000)
+            sleep(0.3)
+
 
 
 

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