-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathautomission.py
136 lines (93 loc) · 3.79 KB
/
automission.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
128
129
130
131
132
133
134
135
from math import sin,cos,radians,pi
class automission(object):
#docstring for automission
def __init__(self,vehicle_type):
super(automission, self).__init__()
assert vehicle_type=='copter'
self.mlist=[] #each element of the array represents a command, ie waypoint, with its parameters
self.counter=1
#these two lines are by default, exists every mission planner file
self.mlist.append('QGC WPL 110\n')
self.mlist.append('0 1 0 0 0 0 0 0 0 0 0 1\n')
def param_to_mcommand(self,*args): #takes command and its parameters, appends them
#to mlist while adjusting formatting
string=str(self.counter)+' '
self.counter+=1
for i in args:
string+=str(i)+' '
string+='\n'
self.mlist.append(string)
### Mission Commands ###
# detailed information about commands and their functionality
# visit http://copter.ardupilot.com/wiki/mission-command-list/#land
################################################################################
#every parameter list begins with '0,3,' and ends with ',1'
def waypoint(self,lat,lon,alt,delay=0):
waypoint_id=16
self.param_to_mcommand(0,3,waypoint_id,delay,0,0,0,lat,lon,alt,1)
def takeoff(self,angle,alt,lat=0,lon=0):
takeoff_id=22
self.param_to_mcommand(0,3,takeoff_id,angle,0,0,0,lat,lon,alt,1)
def land(self,lat=0,lon=0,alt=0):
landid=21
self.param_to_mcommand(0,3,landid,0,0,0,0,lat,lon,alt,1)
def loiter_unlim(self,lat,lon,alt):
loiter_unlimid=17
self.param_to_mcommand(0,3,loiter_unlimid,0,0,0,0,lat,lon,alt,1)
def do_set_roi(self,lat,lon,alt=0):
do_set_roi_id=201
self.param_to_mcommand(0,3,do_set_roi_id,0,0,0,0,lat,lon,alt,1)
def rtl(self):
rtl_id=20
self.param_to_mcommand(0,3,rtl_id,0,0,0,0,0,0,0,1)
def spline_waypoint(self,lat,lon,alt,delay=0):
spline_waypoint_id=82
self.param_to_mcommand(0,3,spline_waypoint_id,delay,0,0,0,lat,lon,alt,1)
def loiter_time(self,time,lat=0,lon=0,alt=0):
loiter_time_id=19
self.param_to_mcommand(0,3,loiter_time_id,time,0,0,0,lat,lon,alt,1)
def loiter_turns(self,turn,direction,lat=0,lon=0,alt=0):
loiter_turns_id=18
self.param_to_mcommand(0,3,loiter_turns_id,turn,0,direction,0,lat,lon,alt,1)
def loiter_unlim(self,lat=0,lon=0,alt=0):
loiter_unlim_id=17
self.param_to_mcommand(0,3,loiter_unlim_id,0,0,0,0,lat,lon,alt,1)
def condition_delay(self,time):
condition_delay_id=112
self.param_to_mcommand(0,3,condition_delay_id,time,0,0,0,0,0,0,1)
def condition_distance(self,dist):
condition_distance_id=114
self.param_to_mcommand(0,3,condition_distance_id,dist,0,0,0,0,0,0,1)
def condition_yaw(self,deg,rel_abs,dir=0):
condition_yaw_id=115
self.param_to_mcommand(0,3,condition_yaw_id,deg,0,dir,rel_abs,0,0,0,1)
def do_jump(self,wp_number,repeat):
do_jump_id=177
self.param_to_mcommand(0,3,do_jump_id,wp_number,repeat,0,0,0,0,0,1)
def do_change_speed(self,speed):
do_change_speed_id=178
self.param_to_mcommand(0,3,do_change_speed_id,speed,0,0,0,0,0,0,1)
def do_set_home(self,current=1,lat=0,lon=0):
do_set_home_id=179
self.param_to_mcommand(0,3,do_set_home_id,current,0,0,0,lat,lon,0,1)
def do_digicam_control(self):
do_digicam_control_id=203
self.param_to_mcommand(0,3,do_digicam_control_id,0,0,0,0,0,0,0,1)
####################################################################################
def write(self,name='output'):
# saves final command list mlist as txt file.
# Missionplanner can direcly open this text document in flight plan / load WP file button
with open(str(name)+".txt", "w") as text_file:
for i in self.mlist:
text_file.write(i)
"""
def meter_to_coord(self,meter,angle,lat,lon):
m_x=meter*cos(radians(angle))
m_y=meter*sin(radians(angle))
R=6378137
yLat = m_y/R
xLon = m_x/(R*cos(radians(lat)))
lat_new= lat + yLat * 180/pi
lon_new=lon+xLon*180/pi
return lat_new,lon_new
"""