-
Notifications
You must be signed in to change notification settings - Fork 1
/
multi_waypoint_real.py
158 lines (137 loc) · 5.61 KB
/
multi_waypoint_real.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
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
import Drone
import sql
from dronekit import connect, VehicleMode, LocationGlobal, LocationGlobalRelative
from pymavlink import mavutil # Needed for command message definitions
import time
import math
from droneapi.lib import Location
import argparse
import g3
import MySQLdb
import camera_control as cc
import Mission
import random
import picamera
import folder_transfer as ftransfer
if __name__ == '__main__':
#pm25 connect
air = g3.g3sensor()
#camera connect
camera = picamera.PiCamera()
#connect to vehicle
parser = argparse.ArgumentParser()
parser.add_argument('--connect', default='/dev/serial0')
args = parser.parse_args()
vehicle = connect(args.connect, baud=57600, wait_ready=True)
#open log file
#connect to mysql
db = MySQLdb.connect(host="120.126.145.102",user="drone",passwd="dronemysql",db="106project")
#get the last mission id that had been done
Id = sql.findResultMaxId(db)
print "Last Task is %s" % Id
#get the final mission object
next_multi_mission = sql.getNextMission(db,str(int(Id)-1))
while True:
sql.closeDatabase(db)
db = MySQLdb.connect(host="120.126.145.102",user="drone",passwd="dronemysql",db="106project")
if sql.findResultMaxId(db) < sql.findMissionMaxId(db):
sql.printTask(db, sql.findResultMaxId(db))
check = raw_input("you want to fly?(Y/n)")
if check is 'Y':
sql_picture_insert = []
waypoint_counter = 0
next_multi_mission = sql.getNextMission(db, next_multi_mission[0].mission_id)
sql.printTask(db, sql.findResultMaxId(db))
Mission_number = next_multi_mission[0].mission_id
print "Mission number is %d" % Mission_number
daytimes = time.strftime("%Y-%m-%d",time.localtime())
logFile=open("./log_file/"+daytimes+"_MISSION_"+str(Mission_number)+".txt","a+")
ftransfer.making_direc(str(Mission_number))
for waypoint_mission in next_multi_mission:
square_count = 1
pm25_sensor = int(waypoint_mission.pm25_sensor)
video_sensor = int(waypoint_mission.video_sensor)
photo_sensor = int(waypoint_mission.photo_sensor)
point_stay = int(waypoint_mission.mission_staytime)
point_height = int(waypoint_mission.mission_height)
waypoint_mission.set_point_num(waypoint_counter+1)
logFile.write("Points "+str(waypoint_counter+1)+":"+"\n")
print "Sensor:"
print "pm2.5:%d video:%d photo:%d" % (pm25_sensor,video_sensor,photo_sensor)
if waypoint_counter == 0:
Drone.arm_and_takeoff(vehicle, point_height)
print "set groundspeed to 5m/s."
vehicle.airspeed = 5
Drone.goto_gps(vehicle,waypoint_mission.mission_latitude, waypoint_mission.mission_longitude, point_height, logFile)
pmdata = air.gsleep(air,point_stay)
waypoint_mission.set_pm25_data(pmdata)
if photo_sensor == 1:
Success = True
try:
Drone.condition_yaw(vehicle,0)
loc = cc.capture(camera,str(Mission_number),str(waypoint_counter+1)+"_"+str(square_count))
#sql.passPhoto(db,waypoint_mission,loc)
sql_picture_insert.append((Mission_number,waypoint_counter+1,loc,"MISSION"+Mission_number+" picture "+waypoint_counter+1))
print "degree 0 , success"
except:
print "except : degree 0"
Success = False
try:
Drone.condition_yaw(vehicle,90)
square_count = square_count+1
loc = cc.capture(camera,str(Mission_number),str(waypoint_counter+1)+"_"+str(square_count))
#sql.passPhoto(db,waypoint_mission,loc)
sql_picture_insert.append((Mission_number,waypoint_counter+1,loc,"MISSION"+Mission_number+" picture "+waypoint_counter+1))
print "degree 90 , success"
except:
print "except : degree 90"
Success = False
try:
Drone.condition_yaw(vehicle,180)
square_count = square_count+1
loc = cc.capture(camera,str(Mission_number),str(waypoint_counter+1)+"_"+str(square_count))
#sql.passPhoto(db,waypoint_mission,loc)
sql_picture_insert.append((Mission_number,waypoint_counter+1,loc,"MISSION"+Mission_number+" picture "+waypoint_counter+1))
print "degree 180 , success"
except:
print "except : degree 180"
Success = False
try:
Drone.condition_yaw(vehicle,270)
square_count = square_count+1
loc = cc.capture(camera,str(Mission_number),str(waypoint_counter+1)+"_"+str(square_count))
#sql.passPhoto(db,waypoint_mission,loc)
sql_picture_insert.append((Mission_number,waypoint_counter+1,loc,"MISSION"+Mission_number+" picture "+waypoint_counter+1))
print "degree 270 , success"
except:
print "except : degree 270"
Success = False
if Success is True:
print " Point %d picture : Finish" % waypoint_counter+1
elif Success is False:
print " Point %d picture : False" % waypoint_counter+1
waypoint_counter += 1
sql.TaskDone(db, next_multi_mission, False)
print "Setting RTL mode..."
vehicle.mode = VehicleMode("RTL")
if photo_sensor ==1:
uploader = raw_input("Task is Done,do you want upload the data ?(Y/n)")
if uploader is 'Y' :
ftransfer.transfer(str(Mission_number))
print "Finish upload.."
for string in sql_picture_insert:
sql.passPhoto(string)
elif uploader is 'n':
print "Skip the upload step..."
elif check is 'n':
next_multi_mission = sql.getNextMission(db, next_multi_mission[0].mission_id)
for waypoint_mission in next_multi_mission:
waypoint_mission.set_pm25_data(0)
sql.TaskDone(db, next_multi_mission, True)
print "last Task is %s" % sql.findResultMaxId(db)
camera.close()
print ""
print "All Task Done !"
sql.closeDatabase(db)
print "Close vehicle object"
vehicle.close()