diff --git a/src/unitdev01/unitdev01/mavlink.py b/src/unitdev01/unitdev01/mavlink.py index b047a0e..9bb6329 100644 --- a/src/unitdev01/unitdev01/mavlink.py +++ b/src/unitdev01/unitdev01/mavlink.py @@ -21,13 +21,15 @@ class MAVLinkWorker(QThread): param_signal = pyqtSignal(str, int) kbps_signal = pyqtSignal(str, float) - def __init__(self, connection_string="udp:127.0.0.1:14550", usb='/dev/ttyUSB0'): + def __init__(self, connection_string="udp:127.0.0.1:14560 ", usb='/dev/ttyUSB0', acm ='/dev/ttyACM0'): super().__init__() - self.namespaces = ['UAV1', 'UAV2', 'UAV3'] - self.connection = mavutil.mavlink_connection(usb, baud=57600) + self.sysids = [1, 5, 10] + self.namespaces = {} + self.namespaces = [f"UAV{sysid}" for sysid in self.sysids] + + self.connection = mavutil.mavlink_connection(connection_string, baud=115200) self.connection.wait_heartbeat() - for sysid in self.namespaces: - sysid = int(sysid.replace('UAV', '')) + for sysid in self.sysids: self.set_sr_params(sysid) self.running = True @@ -42,8 +44,8 @@ class MAVLinkWorker(QThread): self.total_bytes_received = {} self.throughput_KBps = {} - for namespace in self.namespaces: - self.request_param(namespace, "SR1_EXTRA1") + for sysid in self.sysids: + self.request_param(sysid, "SR1_EXTRA1") self.connection.mav.timesync_send( 0, #tc1 int(time.time() * 1e9) #ts1 @@ -52,7 +54,7 @@ class MAVLinkWorker(QThread): def run(self): while self.running: try: - msg = self.connection.recv_msg() + msg = self.connection.recv_msg() # Debugging line to see received messages current_time = time.time() if not msg: continue @@ -64,7 +66,6 @@ class MAVLinkWorker(QThread): if msg_type =="BAD_DATA": continue - if sysid not in self.total_bytes_received: self.total_bytes_received[sysid] = 0 self.throughput_KBps[sysid] = 0 @@ -162,7 +163,7 @@ class MAVLinkWorker(QThread): elif msg_type == 'PARAM_VALUE': param_name = "SR1_EXTRA1" if msg.param_id == param_name: - self.param_signal.emit(namespace, msg.param_value) + self.param_signal.emit(namespace, int(msg.param_value)) except Exception as e: print(f"Error reading message: {e}") @@ -171,8 +172,11 @@ class MAVLinkWorker(QThread): self.running = False self.connection.close() + def get_sysid(self, namespace): + return int(namespace.replace('UAV', '')) + def set_mode(self, namespace, mode): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) if mode == 'STABILIZE': mode = 0 elif mode == 'AUTO': @@ -188,7 +192,7 @@ class MAVLinkWorker(QThread): ) def arm(self, namespace, arm): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # Component ID @@ -199,7 +203,7 @@ class MAVLinkWorker(QThread): ) def takeoff(self, namespace, altitude): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # Component ID @@ -210,7 +214,7 @@ class MAVLinkWorker(QThread): ) def land(self, namespace): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # Component ID @@ -221,7 +225,7 @@ class MAVLinkWorker(QThread): ) def set_local_position(self, namespace, x, y, z): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.set_position_target_local_ned_send( 0, sysid, 1, # time_boot_ms, sysid, compid mavutil.mavlink.MAV_FRAME_LOCAL_NED, # Frame @@ -233,7 +237,7 @@ class MAVLinkWorker(QThread): ) def reboot_drone(self, namespace): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) self.connection.mav.command_long_send( sysid, 1, # target_component (一般為1) @@ -244,7 +248,7 @@ class MAVLinkWorker(QThread): ) def set_param(self, namespace, param_name, value): - sysid = int(namespace.replace('UAV', '')) + sysid = self.get_sysid(namespace) try: self.connection.mav.param_set_send( sysid, @@ -256,8 +260,7 @@ class MAVLinkWorker(QThread): except Exception as e: print(f"Failed to set parameter {param_name}: {e}") - def request_param(self, namespace, param_name): - sysid = int(namespace.replace('UAV', '')) + def request_param(self, sysid, param_name): try: self.connection.mav.param_request_read_send( sysid, # 系統 ID @@ -268,43 +271,57 @@ class MAVLinkWorker(QThread): except Exception as e: print(f"Failed to set parameter {param_name}: {e}") - ''' + def set_sr_params(self, sysid): """ 直接設置 MAVLink 訊息頻率 """ - freqs = [0, 2, 4] + freqs = [0, 1, 4] params = { "SR1_ADSB": freqs[0], "SR1_EXT_STAT": freqs[1], "SR1_EXTRA1": freqs[2], - "SR1_EXTRA2": freqs[2], + "SR1_EXTRA2": freqs[1], "SR1_EXTRA3": freqs[1], - "SR1_POSITION": freqs[1], + "SR1_POSITION": freqs[2], "SR1_RAW_SENS": freqs[1], "SR1_RC_CHAN": freqs[1] } for param, value in params.items(): self.set_param(f"UAV{sysid}", param, value) - ''' + + ''' + def set_sr_params(self, sysid): + """ 直接設置 MAVLink 訊息頻率 """ + params = { + "SERIAL1_BAUD": 38 + } + for param, value in params.items(): + self.set_param(f"UAV{sysid}", param, value) + def set_sr_params(self, sysid): """ 直接設置 MAVLink 訊息頻率 """ - freqs = [0, 1, 1] + freqs = [0, 3, 3] params = { "SR1_ADSB": freqs[0], - "SR1_EXT_STAT": freqs[0], + "SR1_EXT_STAT": freqs[1], "SR1_EXTRA1": freqs[2], "SR1_EXTRA2": freqs[2], - "SR1_EXTRA3": freqs[0], + "SR1_EXTRA3": freqs[1], "SR1_POSITION": freqs[1], "SR1_RAW_SENS": freqs[1], - "SR1_RC_CHAN": freqs[0] + "SR1_RC_CHAN": freqs[1] } for param, value in params.items(): self.set_param(f"UAV{sysid}", param, value) + ''' + def init_drone(self, namespace): + sysid = self.get_sysid(namespace) + self.set_local_position(sysid, 0, 0, 0) class DroneControlApp(QMainWindow): def __init__(self): super().__init__() self.workers = MAVLinkWorker() + self.sysids = self.workers.sysids self.namespaces = self.workers.namespaces self.initUI() @@ -438,6 +455,15 @@ class DroneControlApp(QMainWindow): self.rebootButton.clicked.connect(self.reboot_drone) main_layout.addWidget(self.rebootButton) + mission_layout = QHBoxLayout() + self.initButton = QPushButton("重設位置") + self.initButton.clicked.connect(self.init_drone) + self.startButton = QPushButton("開始任務") + self.startButton.clicked.connect(self.start_mission) + mission_layout.addWidget(self.initButton) + mission_layout.addWidget(self.startButton) + main_layout.addLayout(mission_layout) + # 設置主佈局 central_widget = QWidget(self) central_widget.setLayout(main_layout) @@ -563,6 +589,24 @@ class DroneControlApp(QMainWindow): except ValueError: QtWidgets.QMessageBox.warning(self, "錯誤", "請輸入有效的數值！") + def init_drone(self): + try: + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.init_drone(namespace) + print(f"重設位置：{namespace}") + except Exception as e: + QtWidgets.QMessageBox.warning(self, "錯誤", f"重設位置失敗：{e}") + + def start_mission(self): + try: + for namespace in self.namespaces: + if self.uav_checkboxes[namespace].isChecked(): + self.workers.start_mission(namespace) + print(f"開始任務：{namespace}") + except Exception as e: + QtWidgets.QMessageBox.warning(self, "錯誤", f"開始任務失敗：{e}") + def main(): app = QtWidgets.QApplication(sys.argv) window = DroneControlApp()