Add the log writing function

Signed-off-by: STT

README.md

@@ -41,6 +41,10 @@ python版船控，也可控两轮无人车，作者初学python练手。
 2. Fix the mission upload function
 3. Pack EXE
 
+2021.12.4
+
+1. Add the log writing function 
+
 #### 开发计划
 
 1. 日志功能

log.py

@@ -0,0 +1,55 @@
+import time
+
+
+class Log:
+    """记日志的类"""
+
+    def __init__(self, usv_id):
+        self.file_name = f"AppData/{usv_id}_{time.strftime('%Y_%m_%d_%H_%M')}.csv"
+        with open(self.file_name, 'a') as log:
+            log.write('Timestamp,mode,')
+            log.write('Rudder,Thrust,Status,')
+            log.write('Latitude,Longitude,Altitude,')
+            log.write('Roll,Pitch,Yaw,')
+            log.write('Speed,VelocityNorth,VelocityEast,VelocityDown,')
+            log.write('GyroscopeX,GyroscopeY,GyroscopeZ,')
+            log.write('AccelerometerX,AccelerometerY,AccelerometerZ,')
+            log.write('SystemStatus,FilterStatus,')
+            log.write('command_Rudder,command_Thrust,')
+            log.write('DesiredHeading,DesiredSpeed,DesiredLatitude,DesiredLongitude,')
+            log.write('HeadingP,HeadingI,HeadingD,')
+            log.write('SpeedP,SpeedI,SpeedD,')
+            log.write('PositionP,PositionI,PositionD\n')
+
+    def write_log(self, usv):
+        with open(self.file_name, 'a') as log:
+            log.write(f"{time.strftime('%H_%M_%S')}{str(round(time.time() % 1, 3))[1:]},{usv.control.data['mode']},")
+
+            log.write(f"{usv.control.data['rudder']},{usv.control.data['thrust']},{usv.control.status},")
+
+            log.write(f"{usv.navigation.data['location']['latitude']},{usv.navigation.data['location']['longitude']},")
+            log.write(f"{usv.navigation.data['location']['altitude']},")
+
+            log.write(f"{usv.navigation.data['posture']['roll']},{usv.navigation.data['posture']['pitch']},")
+            log.write(f"{usv.navigation.data['posture']['yaw']},")
+
+            log.write(f"{usv.navigation.data['velocity']['speed']},{usv.navigation.data['velocity']['north']},")
+            log.write(f"{usv.navigation.data['velocity']['east']},{usv.navigation.data['velocity']['down']},")
+
+            log.write(f"{usv.navigation.data['gyroscope']['X']},{usv.navigation.data['gyroscope']['Y']},")
+            log.write(f"{usv.navigation.data['gyroscope']['Z']},")
+
+            log.write(f"{usv.navigation.data['accelerometer']['X']},{usv.navigation.data['accelerometer']['Y']},")
+            log.write(f"{usv.navigation.data['accelerometer']['Z']},")
+
+            log.write(f"{usv.navigation.data['system_status']},{usv.navigation.data['filter_status']},")
+
+            log.write(f"{usv.gcs.command['desired_rudder']},{usv.gcs.command['desired_thrust']},")
+
+            log.write(f"{usv.gcs.command['desired_heading']},{usv.gcs.command['desired_speed']},")
+            log.write(f"{usv.gcs.command['desired_latitude']},{usv.gcs.command['desired_longitude']},")
+
+            log.write(f"{usv.control.pid['heading_p']},{usv.control.pid['heading_i']},{usv.control.pid['heading_d']},")
+            log.write(f"{usv.control.pid['speed_p']},{usv.control.pid['speed_i']},{usv.control.pid['speed_d']},")
+            log.write(
+                f"{usv.control.pid['position_p']},{usv.control.pid['position_i']},{usv.control.pid['position_d']}\n")

mission.py

@@ -1,4 +1,3 @@
-import os
 from xml.etree import ElementTree
 from SelfBuiltModul.func import pretty_xml
 
@@ -6,10 +5,8 @@ from SelfBuiltModul.func import pretty_xml
 class Mission:
     """任务相关类"""
 
-    def __init__(self, id):
-        self.file_name = f"AppData/{id}.xml"
-        if not os.path.exists("AppData"):
-            os.mkdir("AppData")
+    def __init__(self, usv_id):
+        self.file_name = f"AppData/{usv_id}.xml"
 
     def read(self):
         """去读xml文件，返回路点列表"""

settings.py

@@ -1,7 +1,12 @@
+import os
+
+
 class Settings:
     """配置项"""
 
     def __init__(self):
+        if not os.path.exists("AppData"):
+            os.mkdir("AppData")
         self.usv_id = 0
         self.los_distance = 1.5
 

usv.py

@@ -6,6 +6,7 @@ from control import Control
 from settings import Settings
 from ground_control_station import GroundControlStation
 from mission import Mission
+from log import Log
 
 
 class UsvControl:
@@ -19,6 +20,7 @@ class UsvControl:
         self.control = Control()
         self.gcs = GroundControlStation(self.settings.gcs_com)
         self.mission = Mission(self.settings.usv_id)
+        self.log = Log(self.settings.usv_id)
 
     def ms10_run(self):
         self.futaba.rcu_run(self)
@@ -26,6 +28,7 @@ class UsvControl:
         self.navigation.n_run()
         self.gcs.g_run(self)
         self.futaba.backup_data()
+        self.log.write_log(self)
         timer_10 = threading.Timer(0.01, self.ms10_run, )
         timer_10.start()