first commit

报文模拟解析/模拟轨迹/bk.py

@@ -0,0 +1,141 @@
+import socket
+import threading
+import queue
+import random
+import struct
+import time
+import math
+import pyproj
+import vdes_protocol
+
+class vdes_server:
+    def __init__(self, host='localhost', port=10):
+        self.host = host
+        self.port = port
+        self.bufsize = 32768
+        self.addr = (self.host, self.port)
+        self.tcp_send_queue = queue.Queue(32)
+        self.tcp_recv_queue = queue.Queue(16)
+        self.tcp_recv_buffer = bytearray()
+        self.tcp_recv_pointer = 0
+        self.rf_recv_queue = queue.Queue(32)
+        self.rf_recv_buffer = bytearray()
+        self.rf_recv_pointer = 0
+
+        self.tcpCliSock = None
+        self.tcpSerSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.tcpSerSock.bind(self.addr)
+        self.tcpSerSock.listen(2)
+
+        self.my_lon = 121.167
+        self.my_lat = 31.187
+        self.my_heading = 270
+        self.gps_data = vdes_protocol.pkt_gps(self.my_lon, self.my_lat, 0, 0, self.my_heading)
+        # 模拟 40 条船只以本设备为中心 4 ~ 7 海里距离，角度均分，方向随机为离心或向心
+        self.ais_data = list()
+        for i in range(40):
+            radius = random.randint(40, 70)/10*1852 # distributed in 4 ~ 7 nautical miles
+            ship_direction = random.randint(0, 1)
+            ship_heading = (i*9 + ship_direction*180) % 360
+            # Use WGS84 ellipsoid.
+            g = pyproj.Geod(ellps='WGS84')
+            # Forward transformation - returns longitude, latitude, back azimuth of terminus points
+            ship_lon, ship_lat, az = g.fwd(self.my_lon, self.my_lat, ship_heading, radius)
+            ship_mmsi = random.randint(100000000, 999999999)
+            ship_speed = random.randint(14, 30)
+            self.ais_data.append(vdes_protocol.pkt_ais_msg_1(ship_mmsi, ship_speed, ship_lon, ship_lat, ship_heading))
+
+    def recv_parse(self):
+        pass
+
+    def send(self, stop_event):
+        print("send started.")
+        while not stop_event.is_set():
+            if self.tcp_send_queue.empty():
+                time.sleep(0.01)
+                continue
+            data = self.tcp_send_queue.get()
+            if self.tcpCliSock:
+                self.tcpCliSock.send(data)
+
+    def make_response(self, stop_event):
+        print("make_response started.")
+        while not stop_event.is_set():
+            if self.tcp_recv_queue.empty():
+                time.sleep(0.1)
+                continue
+            recv_data += self.tcp_recv_queue.get()
+
+    def make_gps(self, stop_event):
+        print("make_gps started.")
+        while not stop_event.is_set():
+            self.tcp_send_queue.put(self.gps_data.get_packet())
+            self.tcp_send_queue.task_done()
+            time.sleep(1)
+
+    def make_ais(self, stop_event):
+        print("make_ais started.")
+        while not stop_event.is_set():
+            for data in self.ais_data:
+                if stop_event.is_set():
+                    break
+                data = data.get_packet(data.sog * 1852 / 3600 * 2) # 速度放大10倍
+                self.tcp_send_queue.put(data)
+                self.tcp_send_queue.task_done()
+                time.sleep(0.05)
+
+    def start(self):
+        threads = []
+
+        while True:
+            print('Waiting for connection...')
+            self.tcpCliSock, addr = self.tcpSerSock.accept()
+            print('...connected from:', addr)
+
+            stop_event_response = threading.Event()
+            threads.append(threading.Thread(target=self.make_response, args=(stop_event_response,)))
+            stop_event_gps = threading.Event()
+            threads.append(threading.Thread(target=self.make_gps, args=(stop_event_gps,)))
+            stop_event_ais = threading.Event()
+            threads.append(threading.Thread(target=self.make_ais, args=(stop_event_ais,)))
+            stop_event_send = threading.Event()
+            threads.append(threading.Thread(target=self.send, args=(stop_event_send,)))
+
+            for t in threads:
+                t.start()
+            
+            try:
+                while True:
+                    recv_data = self.tcpCliSock.recv(self.bufsize)
+                    if not recv_data:
+                        print("Client disconnected")
+                        break
+                    self.tcp_recv_queue.put(recv_data)
+            except (ConnectionResetError, BrokenPipeError):
+                print("Client forcibly disconnected")
+            except Exception as e:
+                print(f"Error: {e}")
+            finally:
+                self.tcpCliSock.close()
+
+            stop_event_response.set()
+            stop_event_gps.set()
+            stop_event_ais.set()
+            stop_event_send.set()
+            for t in threads:
+                t.join()
+
+            threads.clear()
+            self.tcpCliSock = None
+
+
+if __name__ == "__main__":
+    server = vdes_server()
+    try:
+        server.start()
+    except KeyboardInterrupt:
+        print("Server stopped by user")
+    finally:
+        if server.tcpCliSock:
+            server.tcpCliSock.close()
+        server.tcpSerSock.close()

报文模拟解析/模拟轨迹/vdes_protocol.py

@@ -0,0 +1,163 @@
+import struct
+import datetime
+import random
+import bitarray
+import pyproj
+
+
+class pkt_base:
+    def __init__(self, pkt_type=0, pkt_len=0):
+        self.pkt_header = b'####'
+        self.pkt_type = struct.pack('!I', pkt_type)
+        self.pkt_len = struct.pack('!I', pkt_len)
+        self.pkt_payload = b''
+
+    def get_packet(self):
+        return self.pkt_header + self.pkt_type + self.pkt_len + self.pkt_payload
+    
+
+class pkt_gps(pkt_base):
+    def __init__(self, longitude, latitude, altitude, speed, heading):
+        super().__init__(pkt_type=7)
+        self.gpstime = int(datetime.datetime.now().timestamp() - 315964782) # Timestamp in seconds since 1980.1.6
+        self.lon = int(longitude*10000000)                      # Longitude in degrees * 10^7
+        self.lat = int(latitude*10000000)                       # Latitude in degrees * 10^7
+        self.alt = int(altitude)                                # Altitude in millimeters
+        self.sates_used = random.randint(2, 6)                  # Number of satellites used
+        self.sates_gps = random.randint(2, 6)                   # Number of satellites GPS in view
+        self.sates_bd = random.randint(2, 6)                    # Number of satellites BD in view
+        self.gps_status = random.randint(0, 4)                  # gps status: 0=No fix, 1=2D fix, 2=3D fix, 3=DGPS, 4=RTK float, 5=RTK fixed
+        self.mode = random.randint(1, 3)                        # mode: 1=none, 2=2D, 3=3D
+        self.hdop = random.randint(100, 100000)                 # HDOP (Horizontal Dilution of Precision) in 0.1m
+        self.speed = int(speed*100)                             # Speed in 0.01 knot
+        self.heading = int(heading)                             # Heading in degrees
+        
+    def _fill_payload(self):
+        self.pkt_payload = struct.pack('!QIIIHHHBBIII', 
+                                       self.gpstime, 
+                                       self.lon, 
+                                       self.lat, 
+                                       self.alt, 
+                                       self.sates_used, 
+                                       self.sates_gps, 
+                                       self.sates_bd, 
+                                       self.gps_status, 
+                                       self.mode, 
+                                       self.hdop, 
+                                       self.speed, 
+                                       self.heading)
+
+    def get_packet(self):
+        self._fill_payload()
+        self.pkt_len = struct.pack('!I', len(self.pkt_payload))
+        return super().get_packet()
+
+
+class pkt_demod(pkt_base):
+    def __init__(self):
+        super().__init__(pkt_type=12)
+        self.msg_type = 12                          # 消息类型
+        self.sat_id = random.randint(0, 255)        # 卫星ID
+        self.demod_id = 0                           # 解调器编号
+        self.time_slot = random.randint(10, 2249)   # 解调时隙
+        self.recv_delay = random.randint(0, 65535)  # 接收延迟
+        self.snr = random.randint(-128, 127)        # 信噪比
+
+    def _fill_payload(self):
+        self.pkt_payload = b''
+        self.pkt_payload = struct.pack('!BBBHHh', 
+                              self.msg_type, 
+                              self.sat_id, 
+                              self.demod_id, 
+                              self.time_slot, 
+                              self.recv_delay, 
+                              self.snr)
+
+
+class pkt_ais(pkt_demod):
+    def __init__(self):
+        super().__init__()
+        self.demod_id = random.randint(4, 5)
+
+
+class pkt_ais_msg_1(pkt_ais):
+    def __init__(self, mmsi, sog, lon, lat, heading):
+        super().__init__()
+        self.msg_id = 1                                 # message ID
+        self.repeat_indicator = random.randint(0, 3)    # Repeat indicator
+        self.mmsi = mmsi                                # Maritime Mobile Service Identity
+        self.nav_status = 0                             # Navigation status
+        self.rot = 0                                    # Rate of turn
+        self.sog = int(sog * 10)                        # Speed over ground in 0.1 knot
+        self.pos_acc = random.randint(0, 1)             # Position accuracy
+        self.lon = int(lon * 600000)                    # Longitude in 1/10000 min
+        self.lat = int(lat * 600000)                    # Latitude in 1/10000 min
+        self.cog = int(heading * 10)                    # Course over ground in 0.1 degree
+        self.heading = int(heading)                     # Heading in degrees
+        self.time_stamp = 0                             # Time stamp
+        self.manoeuvre_indicator = random.randint(0, 3) # Manuever indicator
+        self.spare = 0                                  # Spare bits
+        self.raim_flag = random.randint(0, 1)           # RAIM flag
+        self.comm_state = random.randint(0, 3)          # Communication state
+
+    def _fill_payload(self):
+        super()._fill_payload()
+        demod_data  = bitarray.bitarray()
+        demod_data += bitarray.bitarray(f"{self.msg_id:06b}")              # Demodulation flag
+        demod_data += bitarray.bitarray(f"{self.repeat_indicator:02b}")    # Repeat indicator
+        demod_data += bitarray.bitarray(f"{self.mmsi:030b}")               # Maritime Mobile Service Identity
+        demod_data += bitarray.bitarray(f"{self.nav_status:04b}")          # Navigation status
+        demod_data += bitarray.bitarray(f"{self.rot:08b}")                 # Rate of turn
+        demod_data += bitarray.bitarray(f"{self.sog:010b}")                # Speed over ground in 0.1 knot
+        demod_data += bitarray.bitarray(f"{self.pos_acc:01b}")             # Position accuracy
+        demod_data += bitarray.bitarray(f"{self.lon:028b}")                # Longitude in 1/10000 min
+        demod_data += bitarray.bitarray(f"{self.lat:027b}")                # Latitude in 1/10000 min
+        demod_data += bitarray.bitarray(f"{self.cog:012b}")                # Course over ground in 0.1 degree
+        demod_data += bitarray.bitarray(f"{self.heading:09b}")             # Heading in degrees
+        demod_data += bitarray.bitarray(f"{self.time_stamp:06b}")          # Time stamp
+        demod_data += bitarray.bitarray(f"{self.manoeuvre_indicator:02b}") # Manoeuvre indicator
+        demod_data += bitarray.bitarray(f"{self.spare:03b}")               # Spare bits
+        demod_data += bitarray.bitarray(f"{self.raim_flag:01b}")           # RAIM flag
+        demod_data += bitarray.bitarray(f"{self.comm_state:019b}")         # Communication state
+        if len(demod_data) != 168:
+            raise ValueError("ais_msg_1 length is not 168 bits", len(demod_data))
+        self.pkt_payload += demod_data.tobytes()
+        self.pkt_len = struct.pack('!I', len(self.pkt_payload))
+
+    def get_packet(self, distance=0):
+        if distance > 0:
+            # Use WGS84 ellipsoid.
+            g = pyproj.Geod(ellps='WGS84')
+            # Forward transformation - returns longitude, latitude, back azimuth of terminus points
+            lon_new, lat_new, _ = g.fwd(self.lon / 600000, self.lat / 600000, self.cog, distance)
+            self.lon = int(lon_new * 600000)
+            self.lat = int(lat_new * 600000)
+
+        self._fill_payload()
+        return super().get_packet()
+
+
+class pkt_ais_msg_2(pkt_ais_msg_1):
+    def __init__(self, mmsi, sog, lon, lat, heading):
+        super().__init__(mmsi, sog, lon, lat, heading)
+        self.msg_id = 2                                 # message ID
+
+
+class pkt_ais_msg_3(pkt_ais_msg_1):
+    def __init__(self, mmsi, sog, lon, lat, heading):
+        super().__init__(mmsi, sog, lon, lat, heading)
+        self.msg_id = 3                                 # message ID
+
+
+class pkt_vdes(pkt_base):
+    def __init__(self):
+        self.pkt_payload = b''
+
+    def _fill_payload(self):
+        # This method should be overridden in subclasses to fill the payload
+        raise NotImplementedError("Subclasses should implement this method")
+
+    def get_packet(self):
+        self._fill_payload()
+        self.pkt_len = struct.pack('!I', len(self.pkt_payload))
+        return super().get_packet()

报文模拟解析/模拟轨迹/vdes_server.py

@@ -0,0 +1,142 @@
+import socket
+import threading
+import queue
+import random
+import struct
+import time
+import math
+import pyproj
+import vdes_protocol
+
+class vdes_server:
+    def __init__(self, host='localhost', port=10):
+        self.host = host
+        self.port = port
+        self.bufsize = 32768
+        self.addr = (self.host, self.port)
+        self.tcp_send_queue = queue.Queue(32)
+        self.tcp_recv_queue = queue.Queue(16)
+        self.tcp_recv_buffer = bytearray()
+        self.tcp_recv_pointer = 0
+        self.rf_recv_queue = queue.Queue(32)
+        self.rf_recv_buffer = bytearray()
+        self.rf_recv_pointer = 0
+
+        self.tcpCliSock = None
+        self.tcpSerSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.tcpSerSock.bind(self.addr)
+        self.tcpSerSock.listen(2)
+
+        self.my_lon = 122.167
+        self.my_lat = 31.187
+        self.my_heading = 270
+        self.gps_data = vdes_protocol.pkt_gps(self.my_lon, self.my_lat, 0, 0, self.my_heading)
+        # 模拟 40 条船只以本设备为中心 4 ~ 7 海里距离，角度均分，方向随机为离心或向心
+        self.ais_data = list()
+        # Use WGS84 ellipsoid.
+        g = pyproj.Geod(ellps='WGS84')
+        for i in range(40):
+            radius = random.randint(40, 100)/10*1852 # distributed in 4 ~ 7 nautical miles
+            ship_direction = random.randint(0, 1)
+            ship_heading = (i*9 + ship_direction*180) % 360
+            # Forward transformation - returns longitude, latitude, back azimuth of terminus points
+            ship_lon, ship_lat, _ = g.fwd(self.my_lon, self.my_lat, ship_heading, radius)
+            ship_mmsi = random.randint(100000000, 999999999)
+            ship_speed = random.randint(14, 30)
+            self.ais_data.append(vdes_protocol.pkt_ais_msg_1(ship_mmsi, ship_speed, ship_lon, ship_lat, ship_heading))
+
+    def recv_parse(self):
+        pass
+
+    def send(self, stop_event):
+        print("send started.")
+        while not stop_event.is_set():
+            if self.tcp_send_queue.empty():
+                time.sleep(0.01)
+                continue
+            data = self.tcp_send_queue.get()
+            self.tcp_send_queue.task_done()
+            if self.tcpCliSock:
+                self.tcpCliSock.send(data)
+
+    def make_response(self, stop_event):
+        print("make_response started.")
+        while not stop_event.is_set():
+            if self.tcp_recv_queue.empty():
+                time.sleep(0.1)
+                continue
+            recv_data += self.tcp_recv_queue.get()
+
+    def make_gps(self, stop_event):
+        print("make_gps started.")
+        while not stop_event.is_set():
+            self.tcp_send_queue.put(self.gps_data.get_packet())
+            time.sleep(1)
+
+    def make_ais(self, stop_event):
+        print("make_ais started.")
+        while not stop_event.is_set():
+            for data in self.ais_data:
+                if stop_event.is_set():
+                    break
+                data = data.get_packet(data.sog * 1852 / 3600 * 2) # 速度放大10倍
+                self.tcp_send_queue.put(data)
+                time.sleep(0.05)
+
+    def start(self):
+        threads = []
+
+        while True:
+            print('Waiting for connection...')
+            self.tcpCliSock, addr = self.tcpSerSock.accept()
+            print('...connected from:', addr)
+
+            stop_event_response = threading.Event()
+            threads.append(threading.Thread(target=self.make_response, args=(stop_event_response,)))
+            stop_event_gps = threading.Event()
+            threads.append(threading.Thread(target=self.make_gps, args=(stop_event_gps,)))
+            stop_event_ais = threading.Event()
+            threads.append(threading.Thread(target=self.make_ais, args=(stop_event_ais,)))
+            stop_event_send = threading.Event()
+            threads.append(threading.Thread(target=self.send, args=(stop_event_send,)))
+
+            for t in threads:
+                t.start()
+            
+            try:
+                while True:
+                    recv_data = self.tcpCliSock.recv(self.bufsize)
+                    if not recv_data:
+                        print("Client disconnected")
+                        break
+                    self.tcp_recv_queue.put(recv_data)
+            except (ConnectionResetError, BrokenPipeError):
+                print("Client forcibly disconnected")
+            except Exception as e:
+                print(f"Error: {e}")
+            finally:
+                self.tcpCliSock.close()
+
+            stop_event_response.set()
+            stop_event_gps.set()
+            stop_event_ais.set()
+            stop_event_send.set()
+            for t in threads:
+                t.join()
+
+            threads.clear()
+            self.tcpCliSock = None
+
+            #break # TODO: delete when release
+
+
+if __name__ == "__main__":
+    server = vdes_server()
+    try:
+        server.start()
+    except KeyboardInterrupt:
+        print("Server stopped by user")
+    finally:
+        if server.tcpCliSock:
+            server.tcpCliSock.close()
+        server.tcpSerSock.close()