gs works with the GUI

cflib_groundstation/LogfileHandler.py

@@ -46,7 +46,7 @@ class LogfileHandler:
         self.logging_configs = []
         self.data_log = None
         self.data_log_name = ""
-        self.header_id = 0
+        self.header_id = -1
 
         self.create_log_data_file()
 
@@ -75,12 +75,12 @@ class LogfileHandler:
                  'double'   : 0x07}
 
         file = open(filename, "w")
-        file.write(f"{logType} ID   Type    Group   Identifier Name\n")
+        file.write(f"{logType} ID\tType\tGroup\tIdentifier Name\t\n")
         file.close()
         file = open(filename, "a")
         for group in list(toc.keys()):
             for name in list(toc[group].keys()):
-                file.write(f"{toc[group][name].ident}    {types.get(toc[group][name].ctype)}    {group}    {name}\n")
+                file.write(f"{toc[group][name].ident}\t{types.get(toc[group][name].ctype)}\t{group}\t{name}\t\n")
                 #print(f" Identity: {toc[group][name].ident} Packet Type:{toc[group][name].pytype} CType:{toc[group][name].ctype} Group:{group} Name:{name}\n")
 
                    
@@ -133,6 +133,7 @@ class LogfileHandler:
         self.data_log.close()
 
     def add_config_headers(self, config_list: List[LogConfig]):
+        self.header_id += 1
         self.data_log = open(self.data_log_name, 'a')
         header = "#" + str(self.header_id) + "\ttime"
         for config in config_list:
@@ -141,7 +142,6 @@ class LogfileHandler:
                 print("Var: " + variable.name)
                 header += "\t" + variable.name
         self.data_log.write(header + "\r")
-        self.header_id += 1
         self.data_log.close()
 
 
@@ -158,7 +158,7 @@ class LogfileHandler:
         line = str(timestamp) + " "
         for group in data:
             for data in group.values():
-                line += str(data) + "\t"
+                line += str('%.4f'%(data)) + "\t"
         line += "\r"
         self.data_log.write(line)
         self.data_log.close()

cflib_groundstation/crazyflie_connection.py

@@ -54,6 +54,7 @@ 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.timer = QTimer()
@@ -139,8 +140,8 @@ class CrazyflieConnection:
             else :
                 raise Exception
 
-            self.setpoint_handler.setSetpoint(yaw, pitch, roll, thrust)
-            self.setpoint_handler.sendSetpoint()
+            #self.setpoint_handler.setSetpoint(yaw, pitch, roll, thrust)
+            #self.setpoint_handler.sendSetpoint()
 
     def GetNodeIds():
         raise Exception
@@ -256,7 +257,7 @@ class CrazyflieConnection:
         if id == 0: # logdata?
             filename = self.logfile_handler.data_log_name
             data = bytearray()
-            data += bytes(filename, 'utf-8')
+            data += bytes("_" + filename, 'utf-8')
             responsedata = {
             "msg_type": (MessageTypeID.RESPLOGFILE_ID),
             "msg_id": command['msg_id'],
@@ -268,7 +269,7 @@ class CrazyflieConnection:
             params = self.get_param_toc()
             filename = self.logfile_handler.CopyTocToFile(params, True)
             data = bytearray()
-            data += bytes(filename, 'utf-8')
+            data += bytes("_" + filename, 'utf-8')
             responsedata = {
             "msg_type": (MessageTypeID.RESPLOGFILE_ID),
             "msg_id": command['msg_id'],
@@ -280,7 +281,22 @@ class CrazyflieConnection:
             logs = self.get_logging_toc()
             filename = self.logfile_handler.CopyTocToFile(logs, False)
             data = bytearray()
-            data += bytes(filename, 'utf-8')
+            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 == 3: 
+            header = "_" + str(self.logfile_handler.header_id) + ":,time,"
+            for config in self.logging_configs:
+                for variable in config.variables:
+                    header += variable.name + ","
+            print(header)
+            data = bytearray()
+            data += bytes(header, 'utf-8')
             responsedata = {
             "msg_type": (MessageTypeID.RESPLOGFILE_ID),
             "msg_id": command['msg_id'],
@@ -288,10 +304,16 @@ class CrazyflieConnection:
             "data": data
             }
             outputQueue.put(responsedata)
-        elif id == 3: # active header of the data log
-            raise Exception # Not implemented
         elif id == 4: # state of test stand connection
-            raise Exception # Not implemented
+            data = bytearray()
+            data += bytes("_false", 'utf-8')
+            responsedata = {
+            "msg_type": (MessageTypeID.RESPLOGFILE_ID),
+            "msg_id": command['msg_id'],
+            "data_len": len(data),
+            "data": data
+            }
+            outputQueue.put(responsedata)
         else :
             raise Exception
         
@@ -310,24 +332,17 @@ class CrazyflieConnection:
             block_id = command['data'][1]
             self.logging_configs.remove(self.logging_configs[block_id])
         elif id == 4:
-            self.enable_logging()
+            print(4)
+            #self.enable_logging()
         elif id == 5:
-            self.disable_logging()
+            print(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:
-                self.timestamp = log_entry[0]
-                data = log_entry[1]
-                logconf_name = log_entry[2]
-
-                #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
@@ -343,9 +358,10 @@ class CrazyflieConnection:
             self.logging_configs[i].stop()
     
     def delete_log_blocks(self):
-        for block in self.logging_configs:
+        self.logging_configs = []
+        """for block in self.logging_configs:
             block.delete()
-            self.logging_configs.remove(block)
+            self.logging_configs.remove(block)"""
     
     def start_logging(self):
         self.stop_thread = False
@@ -358,11 +374,19 @@ class CrazyflieConnection:
     def continous_log(self):
         while not self.stop_thread:
             data = []
+            print(self.logging_configs)
             for config in self.logging_configs:
-                data.append(self.simple_log(self.scf, config))
+                with SyncLogger(self.scf, config) as logger:
+                    for log_entry in logger:
+
+                        self.timestamp = log_entry[0]
+                        point = log_entry[1]
+                        logconf_name = log_entry[2]
+                        data.append(point)
+                        break
             print(data)
             self.logfile_handler.write_data_points(data, self.timestamp / 1000)
-            sleep(0.3)
+            sleep(0.1)
 
 
 

cflib_groundstation/logs/cflie1_2023_11_15_22:18:22.txt

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

cflib_groundstation/logs/cflie1_2023_11_15_22:18:46.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
84.473 -0.1325208693742752	0.2211969941854477	45.60481262207031	0.03167599067091942	0.036145906895399094	-0.19791899621486664	0.0	0.0	0.0	0.0	0.0	45.6103630065918	
85.512 -0.12650619447231293	0.20479778945446014	45.604732513427734	-0.20262309908866882	-0.04054718837141991	0.10886562615633011	0.0	0.0	0.0	0.0	0.0	45.602203369140625	
86.564 -0.13443784415721893	0.19603952765464783	45.60565948486328	-0.06711671501398087	0.25476938486099243	0.04915372282266617	0.0	0.0	0.0	0.0	0.0	45.607276916503906	
87.602 -0.13041631877422333	0.19279257953166962	45.6108283996582	0.040623195469379425	-0.015579652041196823	-0.21755093336105347	0.0	0.0	0.0	0.0	0.0	45.61414337158203	
88.662 -0.14794139564037323	0.1833554059267044	45.61956024169922	-0.02856205590069294	-0.19934125244617462	0.17758966982364655	0.0	0.0	0.0	0.0	0.0	45.61873245239258	
89.729 -0.14257866144180298	0.19409725069999695	45.62278366088867	0.09397917985916138	0.18471676111221313	0.04588574171066284	0.0	0.0	0.0	0.0	0.0	45.62199020385742	
90.821 -0.14321865141391754	0.20918552577495575	45.62916946411133	0.15656207501888275	-0.3666967749595642	0.10741718113422394	0.0	0.0	0.0	0.0	0.0	45.637359619140625	
91.874 -0.16004642844200134	0.18604686856269836	45.64030456542969	0.14302648603916168	0.042934250086545944	0.025517510250210762	0.0	0.0	0.0	0.0	0.0	45.63774108886719	
92.915 -0.15489603579044342	0.1723555326461792	45.63850402832031	0.0842192992568016	0.1492651104927063	-0.15440456569194794	0.0	0.0	0.0	0.0	0.0	45.63874053955078	
\ No newline at end of file

cflib_groundstation/logs/cflie1_2023_11_15_22:36:01.txt

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

cflib_groundstation/logs/cflie1_2023_11_15_22:36:11.txt

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

cflib_groundstation/logs/cflie1_2023_11_15_22:38:31.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_15_22:39:35.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_15_22:43:41.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_15_22:51:15.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_15_22:51:59.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_15_22:52:47.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_15_22:53:52.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_15_22:54:12.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_15_22:55:14.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_15_22:58:51.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_15_23:05: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.yaw
\ No newline at end of file

cflib_groundstation/logs/cflie1_2023_11_15_23:07:11.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