1. Add 'waypoint' 'trajectory' 'mission' control mode

2. Fix the mission upload function
3. Pack EXE

Signed-off-by: STT

.idea/.name

@@ -1 +1 @@
-settings.py
+usv.py

README.md

@@ -1,6 +1,6 @@
 # USVControl
 
-2021.12.2
+2021.12.3
 
 #### 介绍
 
@@ -32,18 +32,23 @@ python版船控，也可控两轮无人车，作者初学python练手。
 
 2021.12.2
 
-1. add Mission class
+1. Add Mission class
 2. Improve ground control station protocol analysis
 
+2021.12.3
+
+1. Add 'waypoint' 'trajectory' 'mission' control mode
+2. Fix the mission upload function
+3. Pack EXE
+
 #### 开发计划
 
-1. 完善地面站控制相关功能（任务、路点等功能）
-2. 日志功能
-3. 配置文件功能
-4. 代码重构
-5. 共享内存
+1. 日志功能
+2. 配置文件功能
+3. 代码重构
+4. 共享内存
 
-6. 组合导航EKF
+5. 组合导航EKF
 
 
 #### 实验图片

SelfBuiltModul/func.py

@@ -6,6 +6,11 @@ def degrees_to_radians(degrees):
     return degrees / 180 * math.pi
 
 
+def radians_to_degrees(radians):
+    """radians go to degrees"""
+    return radians * 180 / math.pi
+
+
 def to_signed_number(number, max_bytes):
     """Converts a binary number to a signed number"""
     if number < 2 ** (8 * max_bytes - 1):

control.py

@@ -1,8 +1,9 @@
 import math
 import time
 
-from navigation import Navigation
-from SelfBuiltModul.func import degrees_to_radians
+from PySide2.QtPositioning import QGeoCoordinate as Point
+from SelfBuiltModul.func import degrees_to_radians, radians_to_degrees
+from guidance import calculate_los_angle
 
 
 class Control:
@@ -10,13 +11,25 @@ class Control:
         self.data = {'mode': 'lock', 'thrust': 0, 'rudder': 0, 'ignition': 0}
         self.pid = {'heading_p': 500.0, 'heading_i': 0.0, 'heading_d': 0.0, 'speed_p': 0.0, 'speed_i': 0.0,
                     'speed_d': 0.0, 'position_p': 0.0, 'position_i': 0.0, 'position_d': 0.0, 'id': 0x0000}
+        self.point_previous = Point(0.0, 0.0)
+        self.point_current = Point(0.0, 0.0)
+        self.point_desired = Point(0.0, 0.0)
+        self.last_setting = 0
+        self.waypoint_index = 0
         self.rudder_pid = Pid()
         self.thrust_pid = Pid()
+        self.position_pid = Pid()
         self.status = 0  # 无人船状态
         self.depth = 0
         self.battery = 0
 
+    def update(self, usv):
+        self.point_current = Point(usv.navigation.data['location']['latitude'],
+                                   usv.navigation.data['location']['longitude'])
+        self.point_desired = Point(usv.gcs.command['desired_latitude'], usv.gcs.command['desired_longitude'])
+
     def c_run(self, usv):
+        self.update(usv)
         self.choose_mode(usv)
         self.control(usv)
 
@@ -120,16 +133,60 @@ class Control:
         self.data['thrust'] = limit_1000(int(self.control_speed(usv)))
 
     def waypoint(self, usv):  # 5
-        pass
+        if self.point_current.distanceTo(self.point_desired) > usv.settings.gcs_waypoint_err:
+            usv.gcs.command['desired_heading'] = degrees_to_radians(self.point_current.azimuthTo(self.point_desired))
+            self.heading(usv)
+        else:
+            self.lock()
 
     def waypoint_speed(self, usv):  # 6
-        pass
+        if self.point_current.distanceTo(self.point_desired) > usv.settings.gcs_waypoint_err:
+            usv.gcs.command['desired_heading'] = degrees_to_radians(self.point_current.azimuthTo(self.point_desired))
+            self.heading_speed(usv)
+        else:
+            self.lock()
 
     def trajectory_point(self, usv):  # 7
-        pass
+        # 航向
+        desired_heading = degrees_to_radians(self.point_current.azimuthTo(self.point_desired))
+        if abs(usv.gcs.command['desired_heading'] - desired_heading > math.pi) / 2:
+            usv.gcs.command['desired_heading'] = desired_heading + math.pi
+        else:
+            usv.gcs.command['desired_heading'] = desired_heading
+        if usv.gcs.command['desired_heading'] > 2 * math.pi:
+            usv.gcs.command['desired_heading'] -= (2 * math.pi)
+
+        # 航速
+        heading_distance = self.point_current.distanceTo(self.point_desired) * math.cos(
+            desired_heading - usv.navigation.data['posture']['yaw'])
+        usv.gcs.command['desired_speed'] += self.position_pid.calculate_pid(heading_distance, self.pid['position_p'],
+                                                                            self.pid['position_i'],
+                                                                            self.pid['position_d'])
+
+        self.heading_speed(usv)
 
     def mission(self, usv):  # 8
-        pass
+        if self.last_setting != 8:
+            usv.gcs.waypoints = usv.mission.read()
+            self.waypoint_index = 0
+        if len(usv.gcs.waypoints) != 0:
+            way_point = usv.gcs.waypoints[self.waypoint_index]
+            point_next = Point(radians_to_degrees(way_point[0]), radians_to_degrees(way_point[1]))
+            distance = self.point_current.distanceTo(point_next)
+            # 根据容差确定是否要执行下一个点
+            if distance < way_point[2] and self.waypoint_index < len(usv.gcs.waypoints) - 1:
+                self.point_previous = point_next
+                way_point = usv.gcs.waypoints[++self.waypoint_index]
+                point_next.setLatitude(radians_to_degrees(way_point[0]))
+                point_next.setLongitude(radians_to_degrees(way_point[1]))
+            if self.waypoint_index == 0:
+                usv.gcs.command['desired_heading'] = degrees_to_radians(self.point_current.azimuthTo(point_next))
+            else:
+                usv.gcs.command['desired_heading'] = calculate_los_angle(self.point_previous, self.point_current,
+                                                                         point_next, usv.settings.los_distance)
+        self.last_setting = usv.gcs.command['setting']
+
+        self.heading(usv)
 
     def control_heading(self, usv):
         heading_err = usv.gcs.command['desired_heading'] - usv.navigation.data['posture']['yaw']

ground_control_station.py

@@ -1,8 +1,6 @@
 import time
 import serial
 import struct
-from PySide2.QtGui import QPolygon
-import socket
 
 
 class GroundControlStation:
@@ -109,8 +107,8 @@ class GroundControlStation:
                     way_point = struct.unpack('<ddf', bytes(packet_data[11:packet_data_length]))
                     is_same = False
                     if len(self.waypoints) != 0:
-                        if self.waypoints[0] != way_point[0] and self.waypoints[1] != way_point[1] and self.waypoints[
-                                2] != way_point[2]:
+                        if self.waypoints[-1][0] != way_point[0] and self.waypoints[-1][1] != way_point[1] and \
+                                self.waypoints[-1][2] != way_point[2]:
                             is_same = True
                     if not is_same:
                         self.waypoints.append(way_point)
@@ -144,8 +142,8 @@ class GroundControlStation:
         self.gcs.write(send_data)
 
 
-def calculate_header_bytes(id, length, crc16):
-    header_list = [0, id, length, crc16 & 0xff, crc16 // 0x100]
+def calculate_header_bytes(usv_id, length, crc16):
+    header_list = [0, usv_id, length, crc16 & 0xff, crc16 // 0x100]
     header_list[0] = calculate_header_lrc(header_list)
     return struct.pack('<BBBBB', header_list[0], header_list[1], header_list[2], header_list[3], header_list[4])
 

guidance.py

@@ -3,7 +3,7 @@ import math
 from SelfBuiltModul.func import degrees_to_radians
 
 
-def calculate_los_angle(qgc_previous, qgc_current, qgc_next, los_distance):
+def calculate_los_angle(qgc_previous, qgc_current, qgc_next, los_distance=1.5):
     # 直线方向 degrees
     previous_2_next_angle = qgc_previous.azimuthTo(qgc_next)
     # 计算垂点

mission.py

@@ -1,3 +1,4 @@
+import os
 from xml.etree import ElementTree
 from SelfBuiltModul.func import pretty_xml
 
@@ -7,6 +8,8 @@ class Mission:
 
     def __init__(self, id):
         self.file_name = f"AppData/{id}.xml"
+        if not os.path.exists("AppData"):
+            os.mkdir("AppData")
 
     def read(self):
         """去读xml文件，返回路点列表"""
@@ -29,6 +32,4 @@ class Mission:
                                           'tolerance': str(way_point[2])})
         pretty_xml(root, '\t', '\n')
         tree = ElementTree.ElementTree(root)
-        with open(self.file_name):
-            pass
         tree.write(self.file_name, 'UTF-8', True)

settings.py

@@ -2,8 +2,8 @@ class Settings:
     """配置项"""
 
     def __init__(self):
-
         self.usv_id = 0
+        self.los_distance = 1.5
 
         # 串口配置
         self.sbus_com = 'COM50'
@@ -11,6 +11,9 @@ class Settings:
         self.gcs_com = 'COM52'
 
         # 外设配置
+        # 导航
         self.navigation_type = 'wit'
         self.navigation_baudrate = '115200'
+        # 地面站
         self.gcs_disconnect_time_allow = 3
+        self.gcs_waypoint_err = 1