-
Notifications
You must be signed in to change notification settings - Fork 0
/
ppDevice.py
127 lines (112 loc) · 3.51 KB
/
ppDevice.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
import sys
from PySide2.QtWidgets import QApplication, QListWidgetItem, QListWidget
from PySide2.QtWidgets import QVBoxLayout, QPushButton, QWidget, QLabel
from PySide2.QtCore import Signal, Slot, QThread
import time
import inspect
from os.path import join
sys.path.append('./bybop/src')
from Bybop_Discovery import *
import Bybop_Device
def strFlyState(v):
stfs=['LANDED','TAKINGOFF','HOVERING','FLYING','LANDING',
'EMERGENCY','USERTAKEOFF','MOTOR_RAMPPING',
'EMERGENCY_LANDING']
return stfs[v]
class ppDevice(QListWidgetItem):
def __init__(self, device):
self._dev=device
self._instance=None
self._pcmdseq=0
super().__init__(get_name(device))
fullPath = inspect.getfile(ppDevice)
shortPathIndex = fullPath.rfind("/")
if (shortPathIndex == -1):
# handle Windows paths
shortPathIndex = fullPath.rfind("\\")
shortPath = fullPath[0:shortPathIndex]
ip=get_ip(self._dev)
sdpPath = join(shortPath, "sdps")
self._sdpfile = sdpPath+"/"+ip+".sdp"
f= open(self.sdpfile,"w+")
f.write("c=IN IP4 "+ip+"\n")
f.write("m=video 55004 RTP/AVP 96"+"\n")
f.write("a=rtpmap:96 H264/90000"+"\n")
f.close()
@property
def isconnected(self):
return self.drone.is_connected()
@property
def name(self):
return get_name(self._dev)
@property
def ip(self):
return get_ip(self._dev)
@property
def id(self):
return get_device_id(self._dev)
@property
def port(self):
return get_port(self._dev)
@property
def sdpfile(self):
return self._sdpfile
@property
def drone(self):
return self._instance
@property
def flystate(self):
return self.drone.get_flystatus()['state']
@property
def altitude(self):
return self.drone.get_altitude()['altitude']
@property
def roll(self):
return self.drone.get_attitude()['roll']
@property
def pitch(self):
return self.drone.get_attitude()['pitch']
@property
def yaw(self):
return self.drone.get_attitude()['yaw']
@property
def batt(self):
return self.drone.get_battery()
@property
def heading(self):
return self.drone.get_heading()
def connect(self):
d2c_port = 54321
controller_type = "PC"
controller_name = "ppGCS"
print('Connecting to ' + self.name +" ...")
self._instance = Bybop_Device.create_and_connect(
self._dev, d2c_port, controller_type, controller_name)
def start_stream(self):
self.drone.start_streaming()
def stop_stream(self):
self.drone.stop_streaming()
def tkof_lnd_emcy(self):
fs=self.flystate
if fs==0 : # landed
return self.drone.take_off()
elif fs==2 or fs==3: #hovering or flying
return self.drone.land()
elif fs==4: #landing
return self.drone.take_off()
else: # every thing else
return self.drone.emergency()
def land(self):
return self.drone.land()
def rth(self):
return self.drone.RTH()
def circle(self):
return self.drone.circle()
def emcy(self):
return self.drone.emergency()
def PCMD(self,pitch,roll,yaw,gaz):
t=int(round(time.time()*1000))
t=t&0x00FFFFFF
s=self._pcmdseq<<24
tp=s+t
return self.drone.set_PCMD(pitch,roll,yaw,gaz,tp)