wenchun
/
AirTrapMine
forked from chiyu1468/AirTrapMine
1
0
Fork 0
Code Pull Requests Activity

Upload files to 'src/unitdev01/unitdev01'

Browse Source
ken910606 10 months ago
parent 55b77f5920
commit 2783e4ba93
1 changed files with 72 additions and 28 deletions
Show Stats Download Patch File Download Diff File

100
src/unitdev01/unitdev01/mavlink.py
Unescape Escape View File

@ -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()

Write Preview
Loading…
Cancel
Save