Add simulation features

Signed-off-by: STT

ground_control_station.py

@@ -45,7 +45,7 @@ class GroundControlStation:
                 del self.buffer[:packet_length]
                 continue
 
-            elif (packet_data[0] | packet_data[1] << 8) != usv.control.pid['id']:  # 数据过滤
+            elif (packet_data[0] | packet_data[1] << 8) != usv.settings.usv_id:  # 数据过滤
                 del self.buffer[:packet_length]
                 continue
 
@@ -64,7 +64,7 @@ class GroundControlStation:
                 continue
 
             elif packet_id == 2 and packet_data_length == 10:  # 参数请求
-                data_bytes = struct.pack('<Hdfffffffff', usv.control.pid['id'], time.time(),
+                data_bytes = struct.pack('<Hdfffffffff', usv.settings.usv_id, time.time(),
                                          usv.control.pid['heading_p'], usv.control.pid['heading_i'],
                                          usv.control.pid['heading_d'], usv.control.pid['speed_p'],
                                          usv.control.pid['speed_i'], usv.control.pid['speed_d'],
@@ -91,7 +91,7 @@ class GroundControlStation:
                 usv.control.pid['position_i'] = pid[7]
                 usv.control.pid['position_d'] = pid[8]
                 # 设置回应
-                data_bytes = struct.pack('<HdB', usv.control.pid['id'], time.time(), 0xff)
+                data_bytes = struct.pack('<HdB', usv.settings.usv_id, time.time(), 0xff)
                 crc16 = calculate_crc16_ccitt(data_bytes, len(data_bytes))
                 header_bytes = calculate_header_bytes(18, 11, crc16)  # 回应包id=18，包长11
                 send_data = header_bytes + data_bytes
@@ -115,7 +115,7 @@ class GroundControlStation:
                 if packet_data_length == 11 and data_type == 2:
                     usv.mission.write(self.waypoints)
                 # 返回成功信号
-                data_bytes = struct.pack('<HdB', usv.control.pid['id'], time.time(), 0xff)
+                data_bytes = struct.pack('<HdB', usv.settings.usv_id, time.time(), 0xff)
                 crc16 = calculate_crc16_ccitt(data_bytes, len(data_bytes))
                 header_bytes = calculate_header_bytes(19, 11, crc16)  # 回应包id=19，包长11
                 send_data = header_bytes + data_bytes
@@ -124,7 +124,7 @@ class GroundControlStation:
                 continue
 
     def send_status(self, usv):
-        data_bytes = struct.pack('<HdBdddffffffffffffHHffhhb', usv.control.pid['id'], time.time(), usv.control.status,
+        data_bytes = struct.pack('<HdBdddffffffffffffHHffhhb', usv.settings.usv_id, time.time(), usv.control.status,
                                  usv.navigation.data['location']['latitude'],
                                  usv.navigation.data['location']['longitude'],
                                  usv.navigation.data['location']['altitude'], usv.navigation.data['posture']['roll'],

navigation.py

@@ -1,8 +1,12 @@
 import math
+import struct
+
 import serial
 import time
+import socket
 
 from SelfBuiltModul.func import degrees_to_radians, to_signed_number
+from ground_control_station import calculate_header_lrc, calculate_crc16_ccitt
 
 
 class Navigation:
@@ -15,14 +19,17 @@ class Navigation:
                      'gyroscope': {'X': 0.0, 'Y': 0.0, 'Z': 0.0},
                      'accelerometer': {'X': 0.0, 'Y': 0.0, 'Z': 0.0}, 'timestamp': 0.0, 'errors': 0, 'system_status': 0,
                      'filter_status': 0}
-        self.navigation = serial.Serial(com, baudrate, timeout=0, write_timeout=0)
         self.buffer = []
         self.packet = []
         self.navigation_type = navigation_type
+        if self.navigation_type == 'wit':
+            self.navigation = serial.Serial(com, baudrate, timeout=0, write_timeout=0)
 
-    def n_run(self):
+    def n_run(self, usv):
         if self.navigation_type == 'wit':
             self.wit_decode()
+        elif self.navigation_type == 'airsim':
+            self.airsim_decode(usv)
 
     def wit_decode(self):
         """接受维特组合导航数据并解析"""
@@ -97,6 +104,60 @@ class Navigation:
         """打包维特导航数据"""
         pass
 
+    def airsim_decode(self, usv):
+        buffer = []
+        ip_port = (usv.settings.usv_ip, usv.settings.airsim_port)
+        receive = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        receive.bind(ip_port)
+        data, send_add = receive.recvfrom(82)
+
+        if send_add == (usv.settings.airsim_ip, usv.settings.airsim_port):
+            buffer += data
+
+        while len(buffer) >= 5:
+            if self.buffer[0] != calculate_header_lrc(self.buffer):
+                del self.buffer[0]
+                continue
+
+            packet_data_length = self.buffer[2]
+            packet_length = packet_data_length + 5
+
+            if len(self.buffer) < packet_length:
+                break
+            if (self.buffer[3] | self.buffer[4] << 8) != calculate_crc16_ccitt(self.buffer[5:], packet_data_length):
+                del self.buffer[0]
+                self.data['errors'] += 1
+                continue
+
+            packet_id = self.buffer[1]
+            packet_data = self.buffer[5:packet_length]
+
+            if (packet_data[0] | packet_data[1] << 8) != usv.control.pid['id']:  # 数据过滤
+                del self.buffer[:packet_length]
+                continue
+
+            elif packet_id == 10 and packet_data_length == 82:  # Command
+                command = struct.unpack('<dddffffffffffff', bytes(packet_data[10:packet_data_length]))
+                self.data['timestamp'] = time.time()
+                self.data['location']['latitude'] = command[0]
+                self.data['location']['longitude'] = command[1]
+                self.data['location']['altitude'] = command[2]
+                self.data['posture']['pitch'] = command[3]
+                self.data['posture']['roll'] = command[4]
+                self.data['posture']['yaw'] = command[5]
+                self.data['velocity']['north'] = command[6]
+                self.data['velocity']['east'] = command[7]
+                self.data['velocity']['down'] = command[8]
+                self.data['gyroscope']['Y'] = command[9]
+                self.data['gyroscope']['X'] = command[10]
+                self.data['gyroscope']['Z'] = command[11]
+                self.data['accelerometer']['Y'] = command[12]
+                self.data['accelerometer']['X'] = command[13]
+                self.data['accelerometer']['Z'] = command[14]
+                del self.buffer[:packet_length]
+                continue
+        receive.close()
+
     @staticmethod
     def wit_check_sum(data):
         return (data[0] + data[1] + data[2] + data[3] + data[4] + data[5] + data[6] + data[7] + data[8] + data[

remote_control_unit.py

@@ -1,5 +1,10 @@
+import socket
+import struct
+import time
+
 import serial
 import array
+from ground_control_station import calculate_header_bytes, calculate_crc16_ccitt
 
 
 class RemoteControlUnit:
@@ -22,14 +27,25 @@ class RemoteControlUnit:
         self.send_command(usv)
 
     def send_command(self, usv):
-        self.send_data = self.receive_data.copy()
-        self.send_data['channel1'] = int(
-            1024 + ((usv.control.data['rudder'] + usv.control.data['thrust']) * 0.672))  # 左电机
-        self.send_data['channel3'] = int(
-            1024 + ((usv.control.data['thrust'] - usv.control.data['rudder']) * 0.672))  # 右电机
+        if usv.settings.navigation_type != "airsim":
+            self.send_data = self.receive_data.copy()
+            self.send_data['channel1'] = int(
+                1024 + ((usv.control.data['rudder'] + usv.control.data['thrust']) * 0.672))  # 左电机
+            self.send_data['channel3'] = int(
+                1024 + ((usv.control.data['thrust'] - usv.control.data['rudder']) * 0.672))  # 右电机
 
-        self.encode()
-        self.sbus.write(self.packet.tobytes())
+            self.encode()
+            self.sbus.write(self.packet.tobytes())
+        else:
+            data_bytes = struct.pack('<Bdhh', usv.settings.usv_id, time.time(), usv.control.data['rudder'],
+                                     usv.control.data['thrust'])
+            crc16 = calculate_crc16_ccitt(data_bytes, len(data_bytes))
+            header_bytes = calculate_header_bytes(0, 16, crc16)  # 回应包id=0，包长16
+            send_data = header_bytes + data_bytes
+            ip_port = (usv.settings.airsim_ip, usv.settings.airsim_port)
+            send = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+            send.sendto(send_data, ip_port)
+            send.close()
 
     def decode(self, ):
         """读取数据并解包数据"""

settings.py

@@ -8,6 +8,7 @@ class Settings:
         if not os.path.exists("AppData"):
             os.mkdir("AppData")
         self.usv_id = 0
+        self.usv_ip = '192.2.5.53'
         self.los_distance = 1.5
 
         # 串口配置
@@ -17,8 +18,12 @@ class Settings:
 
         # 外设配置
         # 导航
-        self.navigation_type = 'wit'
+        self.navigation_type = 'airsim'
         self.navigation_baudrate = '115200'
+        self.airsim_ip = '192.168.31.86'
+        self.airsim_port = 9876
+
+
         # 地面站
         self.gcs_disconnect_time_allow = 3
         self.gcs_waypoint_err = 1

usv.py

@@ -25,7 +25,7 @@ class UsvControl:
     def ms10_run(self):
         self.futaba.rcu_run(self)
         self.control.c_run(self)
-        self.navigation.n_run()
+        self.navigation.n_run(self)
         self.gcs.g_run(self)
         self.futaba.backup_data()
         self.log.write_log(self)