遥控板初版

.idea/.gitignore

@@ -0,0 +1,3 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml

.idea/.name

@@ -0,0 +1 @@
+usv.py

.idea/USVControl.iml

@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$">
+      <excludeFolder url="file://$MODULE_DIR$/venv" />
+    </content>
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>

.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.9 (USVControl)" project-jdk-type="Python SDK" />
+</project>

.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/USVControl.iml" filepath="$PROJECT_DIR$/.idea/USVControl.iml" />
+    </modules>
+  </component>
+</project>

control.py

@@ -0,0 +1,13 @@
+import struct
+from remote_control_unit import RemoteControlUnit
+
+
+class Control:
+    def __init__(self):
+        self.mode = ['lock', 'manual']
+        self.data = {'mode': self.mode[1], 'thrust': 0, 'rudder': 0, }
+
+    def run(self, usv):
+        if self.data['mode'] == self.mode[1]:
+            self.data['rudder'] = int(1.4881 * (usv.futaba.receive_data['channel1'] - 1024))
+            self.data['thrust'] = int(-1.4881 * (usv.futaba.receive_data['channel3'] - 1024))

navigation.py

@@ -0,0 +1,5 @@
+class Navigation:
+    """组合导航模块"""
+
+    def __init__(self):
+        pass

remote_control_unit.py

@@ -0,0 +1,98 @@
+import serial
+import array
+
+
+class RemoteControlUnit:
+    """遥控器相关属性及方法"""
+
+    def __init__(self, com, ):
+        self.receive_data = {'channel1': 0, 'channel2': 0, 'channel3': 0, 'channel4': 0, 'channel5': 0, 'channel6': 0,
+                             'channel7': 0, 'channel8': 0, 'channel9': 0, 'channel10': 0, 'channel11': 0,
+                             'channel12': 0, 'channel13': 0, 'channel14': 0, 'channel15': 0, 'channel16': 0, 'flag': 0,
+                             'error': 0, }
+        self.send_data = self.receive_data.copy()
+        self.sbus = serial.Serial(com, 100000, serial.EIGHTBITS, serial.PARITY_EVEN, serial.STOPBITS_TWO, 0, False,
+                                  False, 0)
+        self.buffer = []
+        self.packet = array.array('B')
+
+    def run(self, usv):
+        self.buffer += self.sbus.read()
+        self.decode(usv)
+
+    def decode(self, usv):
+        """解包数据"""
+        while len(self.buffer) >= 25:
+            if self.buffer[0] != 0x0f:
+                del self.buffer[0]
+                continue
+
+            if self.buffer[24] != 0x04 and self.buffer[24] != 0x14 and self.buffer[24] != 0x24 and self.buffer[
+                24] != 0x34:
+                del self.buffer[0]
+                self.receive_data['error'] += 1
+                continue
+
+            self.receive_data['channel1'] = (self.buffer[1] >> 0 | self.buffer[2] << 8) & 0x07ff
+            self.receive_data['channel2'] = (self.buffer[2] >> 3 | self.buffer[3] << 5) & 0x07ff
+            self.receive_data['channel3'] = (self.buffer[3] >> 6 | self.buffer[4] << 2 |
+                                             self.buffer[5] << 10) & 0x07ff
+            self.receive_data['channel4'] = (self.buffer[5] >> 1 | self.buffer[6] << 7) & 0x07ff
+            self.receive_data['channel5'] = (self.buffer[6] >> 4 | self.buffer[7] << 4) & 0x07ff
+            self.receive_data['channel6'] = (self.buffer[7] >> 7 | self.buffer[8] << 1 |
+                                             self.buffer[9] << 9) & 0x07ff
+            self.receive_data['channel7'] = (self.buffer[9] >> 2 | self.buffer[10] << 6) & 0x07ff
+            self.receive_data['channel8'] = (self.buffer[10] >> 5 | self.buffer[11] << 3) & 0x07ff
+            self.receive_data['channel9'] = (self.buffer[12] >> 0 | self.buffer[13] << 8) & 0x07ff
+            self.receive_data['channel10'] = (self.buffer[13] >> 3 | self.buffer[14] << 5) & 0x07ff
+            self.receive_data['channel11'] = (self.buffer[14] >> 6 | self.buffer[15] << 2 |
+                                              self.buffer[16] << 10) & 0x07ff
+            self.receive_data['channel12'] = (self.buffer[16] >> 1 | self.buffer[17] << 7) & 0x07ff
+            self.receive_data['channel13'] = (self.buffer[17] >> 4 | self.buffer[18] << 4) & 0x07ff
+            self.receive_data['channel14'] = (self.buffer[18] >> 7 | self.buffer[19] << 1 |
+                                              self.buffer[20]) << 9 & 0x07ff
+            self.receive_data['channel15'] = (self.buffer[20] >> 2 | self.buffer[21] << 6) & 0x07ff
+            self.receive_data['channel16'] = (self.buffer[21] >> 5 | self.buffer[22] << 3) & 0x07ff
+            self.receive_data['flag'] = self.buffer[23]
+
+            del self.buffer[:25]
+
+            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())
+
+            break
+
+    def encode(self):
+        """打包数据"""
+        self.packet = array.array('B')
+        self.packet.append(0x0f)
+        self.packet.append(self.send_data['channel1'] & 0xff)
+        self.packet.append((self.send_data['channel1'] >> 8 | self.send_data['channel2'] << 3) & 0xff)
+        self.packet.append((self.send_data['channel2'] >> 5 | self.send_data['channel3'] << 6) & 0xff)
+        self.packet.append((self.send_data['channel3'] >> 2) & 0xff)
+        self.packet.append((self.send_data['channel3'] >> 10 | self.send_data['channel4'] << 1) & 0xff)
+        self.packet.append((self.send_data['channel4'] >> 7 | self.send_data['channel5'] << 4) & 0xff)
+        self.packet.append((self.send_data['channel5'] >> 4 | self.send_data['channel6'] << 7) & 0xff)
+        self.packet.append((self.send_data['channel6'] >> 1) & 0xff)
+        self.packet.append((self.send_data['channel6'] >> 9 | self.send_data['channel7'] << 2) & 0xff)
+        self.packet.append((self.send_data['channel7'] >> 6 | self.send_data['channel8'] << 5) & 0xff)
+        self.packet.append((self.send_data['channel8'] >> 3) & 0xff)
+        self.packet.append(self.send_data['channel9'] & 0xff)
+        self.packet.append((self.send_data['channel9'] >> 8 | self.send_data['channel10'] << 3) & 0xff)
+        self.packet.append((self.send_data['channel10'] >> 5 | self.send_data['channel11'] << 6) & 0xff)
+        self.packet.append((self.send_data['channel11'] >> 2) & 0xff)
+        self.packet.append((self.send_data['channel11'] >> 10 | self.send_data['channel12'] << 1) & 0xff)
+        self.packet.append((self.send_data['channel12'] >> 7 | self.send_data['channel13'] << 4) & 0xff)
+        self.packet.append((self.send_data['channel13'] >> 4 | self.send_data['channel14'] << 7) & 0xff)
+        self.packet.append((self.send_data['channel14'] >> 1) & 0xff)
+        self.packet.append((self.send_data['channel14'] >> 9 | self.send_data['channel15'] << 2) & 0xff)
+        self.packet.append((self.send_data['channel15'] >> 6 | self.send_data['channel16'] << 5) & 0xff)
+        self.packet.append((self.send_data['channel16'] >> 3) & 0xff)
+        self.packet.append(self.send_data['flag'])
+        self.packet.append(0x04)

settings.py

@@ -0,0 +1,6 @@
+class Settings:
+    """配置项"""
+
+    def __init__(self):
+        # 串口配置
+        self.sbus_com = 'COM50'

usv.py

@@ -0,0 +1,23 @@
+from remote_control_unit import RemoteControlUnit
+from control import Control
+from settings import Settings
+
+
+class UsvControl:
+    """程序入口"""
+
+    def __init__(self):
+        self.settings = Settings()
+        self.futaba = RemoteControlUnit(self.settings.sbus_com)
+        self.control = Control()
+
+    def run(self):
+        while True:
+            self.futaba.run(self)
+            self.control.run(self)
+
+
+# 按间距中的绿色按钮以运行脚本。
+if __name__ == '__main__':
+    usv1 = UsvControl()
+    usv1.run()