def landSet(): descent_ratio = 17.0/150.0 #20 meter descent for every 150 meters traveled alt1 = 150 alt2 = 65 alt3 = 36 bear = landbearing dist1 = 800 dist2 = 550 dist3 = 350 pre1 = [dist1*cos(bear),dist1*sin(bear),alt1] pre2 = [dist2*cos(bear),dist2*sin(bear),alt2] pre3 = [dist3*cos(bear),dist3*sin(bear),alt3] pre1 = cord_System.toGPS(pre1) pre2 = cord_System.toGPS(pre2) pre3 = cord_System.toGPS(pre3) pre1 = Locationwp().Set(pre1[0],pre1[1],alt1, 16) pre2 = Locationwp().Set(pre2[0],pre2[1],alt2, 16) pre3 = Locationwp().Set(pre3[0],pre3[1],alt3, 16) landing = Locationwp().Set(Home[0],Home[1],0,21) return [pre1,pre2,pre3,landing]
def prepare(self, checkForAbort=True, lat=35.490163, lng=24.064234):
idmavcmd = MAVLink.MAV_CMD.WAYPOINT
id = int(idmavcmd)
#lat = 35.490163
#lng = 24.064234
home = Locationwp().Set(lat, lng, 0, id)
print "upload home - reset on arm"
waypoints = []
#for i in range(0, len(self.commands)):# self._getUncompetedSize()):
# command = self._getNextCommand()
i = 1
for command in self.commands:
if command.command == "WAYPOINT":
waypoint = Locationwp().Set(float(command.latitude),
float(command.longitude), 0, id)
waypoints.append((i, waypoint))
i = i + 1
print "set wp total ", len(waypoints)
MAV.setWPTotal(len(waypoints) + 1)
MAV.setWP(home, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
for waypoint in waypoints:
print "seting waypoint ", waypoint[1], waypoint[0]
MAV.setWP(waypoint[1], waypoint[0],
MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
MAV.setWPACK()
def emergentwpsSet(): lst = [] lst.append(Locationwp().Set(emergentloc[0],emergentloc[1],emergentloc[2], 16)) cam = Locationwp() Locationwp.id.SetValue(cam, 203) # lst.append(cam) return lst
def cameraGrid(wpfileLoc):
lngth = file_len_Loc(wpfileLoc)
with open(wpfileLoc, "r") as globallist:
lst = []
if (globallist.readline().split(" ")[1] != "WPL"):
print "Nothing found"
else:
globallist.readline()
i = 0
for wpnum in range(lngth - 2):
dat = globallist.readline().split(" ")
if (int(dat[3]) == 16):
lst.append(Locationwp().Set(float(dat[8]), float(dat[9]),
float(dat[10]), 16))
elif (int(dat[3]) == 203):
cam = Locationwp()
Locationwp.id.SetValue(cam, 203)
Locationwp.p1.SetValue(cam, float(dat[4]))
Locationwp.p2.SetValue(cam, float(dat[5]))
Locationwp.p3.SetValue(cam, float(dat[6]))
Locationwp.p4.SetValue(cam, float(dat[7]))
Locationwp.lat.SetValue(cam, float(dat[8]))
Locationwp.lng.SetValue(cam, float(dat[9]))
Locationwp.alt.SetValue(cam, float(dat[10]))
lst.append(cam) #append cam trig
elif (int(dat[3]) == 206):
cam = Locationwp()
Locationwp.id.SetValue(cam, 206)
Locationwp.p1.SetValue(cam, float(dat[4]))
Locationwp.p2.SetValue(cam, float(dat[5]))
Locationwp.p3.SetValue(cam, float(dat[6]))
Locationwp.p4.SetValue(cam, float(dat[7]))
Locationwp.lat.SetValue(cam, float(dat[8]))
Locationwp.lng.SetValue(cam, float(dat[9]))
Locationwp.alt.SetValue(cam, float(dat[10]))
lst.append(cam) #append cam trig
else:
print "was not 16, 206, or 203"
i = wpnum
print "Search grid number of waypoints are", len(lst)
#end it here and return the list
writePhotoLocations(lst)
return lst
def getMissionData(missionFileLoc):
with open(missionFileLoc, "r") as missionFile:
airdropData = missionFile.readline().split(" ")
homeData = missionFile.readline().split(" ")
offaxisData = missionFile.readline().split(" ")
emergentData = missionFile.readline().split(" ")
airdropLoc = Locationwp().Set(float(airdropData[1]),
float(airdropData[2]), 0, 16)
homeLoc = Locationwp().Set(float(homeData[1]), float(homeData[2]), 0,
16)
offaxistagetLoc = Locationwp().Set(float(offaxisData[1]),
float(offaxisData[2]), 0, 16)
emergentLoc = Locationwp().Set(float(emergentData[1]),
float(emergentData[2]), 0, 16)
return airdropLoc, homeLoc, offaxistagetLoc, emergentLoc
def land():
descent_ratio = 20.0 / 150.0 #20 meter descent for every 150 meters traveled
alt1 = 60
alt2 = 50
dist1 = gpsDist(alt1 / descent_ratio)
dist2 = gpsDist(alt2 / descent_ratio)
lat1 = Dstar.Home.lat - dist1 * sin(landbear)
lng1 = Dstar.Home.lng - dist1 * cos(landbear)
lat2 = Dstar.Home.lat - dist2 * sin(landbear)
lng2 = Dstar.Home.lng - dist2 * cos(landbear)
return [
Locationwp().Set(lat1, lng1, alt1, 16),
Locationwp().Set(lat2, lng2, alt1, 16),
Locationwp().Set(Dstar.Home.lat, Dstar.Home.lng, 0, 21)
]
def getTakeoffLoc(loc, bear):
dist = 310
lat1 = loc.lat + delLat * dist * sin(bear)
lng1 = loc.lng + delLng * dist * cos(bear)
to = Locationwp().Set(lat1, lng1, 100, 16)
return to
def travel(previousWps, start, goal):
wp = cs.wpno
temp = []
if (start != None):
temp.append(start)
temp.extend(previousWps)
temp.append(goal)
upload(temp, 1)
if (start != None):
st = start
while (cs.wpno == wp):
Script.Sleep(10)
else:
st = Locationwp().Set(cs.lat, cs.lng, cs.alt, 16)
path = getPath(st, goal, True, 4)
actualpath = temp
if (path != False):
path.append(goal)
actualpath = path
upload(path, 1)
wp = 1
while (cs.wpno < len(actualpath) - 1):
Script.Sleep(50)
if (cs.wpno != wp):
wp = cs.wpno
print strng, " Wp number ", wp
def getWPData(WPFileLoc):
length = file_len_Loc(WPFileLoc)
WPs = []
with open(WPFileLoc, "r") as WPFile:
for i in range(length):
WPdat = WPFile.readline().split(" ")
WPs.append(Locationwp().Set(float(WPdat[1]), float(WPdat[2]),
float(WPdat[3]), 16))
return WPs
def payloaddrop(loc, bear):
dist = 20 #function of alt and vel
windoffset = 0
wind = gpsDist(windoffset)
windbearing = 0
dropAlt = 50
vel = 16
dist1 = gpsDist(dist)
droplat = loc.lat - dist1 * sin(bear) - wind * sin(windbearing)
droplng = loc.lng - dist1 * cos(bear) - wind * cos(windbearing)
distprevious1 = gpsDist(220)
prelat1 = loc.lat - distprevious1 * sin(bear) - wind * sin(windbearing)
prelng1 = loc.lng - distprevious1 * cos(bear) - wind * cos(windbearing)
distprevious2 = gpsDist(80)
prelat2 = loc.lat - distprevious2 * sin(bear) - wind * sin(windbearing)
prelng2 = loc.lng - distprevious2 * cos(bear) - wind * cos(windbearing)
distafter = gpsDist(50)
postlat = loc.lat + distafter * sin(bear) - wind * sin(windbearing)
postlng = loc.lng + distafter * cos(bear) - wind * cos(windbearing)
#speak("setting up payload drop")
#setHome(50)
loc1 = Locationwp().Set(prelat1, prelng1, 50, 16)
#MAV.setWPCurrent(1)
loc2 = Locationwp().Set(prelat2, prelng2, 50, 16)
loc3 = Locationwp().Set(droplat, droplng, 50, 16)
dropWP = Locationwp()
Locationwp.id.SetValue(dropWP, 183)
Locationwp.p1.SetValue(dropWP, 5)
Locationwp.p2.SetValue(dropWP, 1100)
loc5 = Locationwp().Set(postlat, postlng, 50, 16)
return [loc1, loc2, loc3, dropWP, loc5]
'''
def checkaltitudechange(loc1, loc2): #,loiterLoc):
wps = []
for i in range(len(lst) - 1):
dist = gps_distance(loc1.lat, loc1.lng, loc2.lat, loc2.lng)
height = loc2.alt - loc1.alt
slope = height / dist
if (slope <= .48 or slope >= -.48):
return False
else:
return Locationwp().Set((loc2.lat + loc1.lat) / 2,
(loc2.lng + loc1.lng) / 2, loc2.alt, 31)
def emergentTarget(safeloc, direc):
height = 100
predist1 = -60
latPre1 = safeloc.lat + (predist1 * sin(direc)) * delLat
lngPre1 = safeloc.lng + (predist1 * cos(direc)) * delLng
preOff1 = Locationwp().Set(latPre1, lngPre1, height, 16)
predist2 = -50
latPre2 = safeloc.lat + (predist2 * sin(direc)) * delLat
lngPre2 = safeloc.lng + (predist2 * cos(direc)) * delLng
preOff2 = Locationwp().Set(latPre2, lngPre2, height, 16)
takephoto = Locationwp()
Locationwp.id.SetValue(takephoto, 203)
afterDist = 40
lataft = safeloc.lat + (afterDist * sin(direc)) * delLat
lngaft = safeloc.lng + (afterDist * cos(direc)) * delLng
afterOff = Locationwp().Set(lataft, lngaft, height, 16)
return [preOff1, preOff1, takephoto, afterOff]
def landLoc(loc, bear):
descent_ratio = 20.0 / 150.0 #20 meter descent for every 150 meters traveled
alt1 = 40
alt2 = 30
dist1 = gpsDist(alt1 / descent_ratio)
dist2 = gpsDist(alt2 / descent_ratio)
dist0 = gpsDist(alt1 / descent_ratio + 20)
lat1 = loc.lat - dist1 * sin(bear)
lng1 = loc.lng - dist1 * cos(bear)
lat2 = loc.lat - dist2 * sin(bear)
lng2 = loc.lng - dist2 * cos(bear)
lat0 = loc.lat - dist0 * sin(bear)
lng0 = loc.lng - dist0 * cos(bear)
loc0 = Locationwp().Set(lat0, lng0, alt2, 31)
loc1 = Locationwp().Set(lat1, lng1, alt1, 16)
loc2 = Locationwp().Set(lat2, lng2, alt2, 16)
loc3 = Locationwp().Set(loc.lat, loc.lng, 0, 21)
return [loc0, loc1, loc2, loc3]
def findSafeLoiter(lst):
dist = dist_loc(lst[0], lst[1])
dlat = (lst[1].lat - lst[0].lat) / int(dist / 10)
dlng = (lst[1].lng - lst[0].lng) / int(dist / 10)
for j in range(int(dist / 10)):
loc = Locationwp().Set(lst[0].lat + j * dlat,
lst[0].lng + j * dlat, alt, 16)
if (SafetoLoiter(loc)):
return loc
#not sure how to find better point
print "no point found that is safe to loiter"
return lst[0]
def getBoundryData(BoundFileLoc):
length = file_len_Loc(BoundFileLoc)
bounds = []
with open(BoundFileLoc, "r") as BoundFile:
BoundFile.readline()
BoundFile.readline()
print "opened"
for i in range(length - 2):
WPdat = BoundFile.readline().split(" ")
print WPdat
bounds.append(Locationwp().Set(float(WPdat[1]), float(WPdat[2]), 0,
16))
return bounds
def findSafeOffAxisLoc(boundry1, boundry2, loc):
DistFromLine = 75
gl = [boundry2.lng, boundry2.lat, 0]
st = [boundry1.lng, boundry1.lat, 0]
ob = [loc.lng, loc.lat, 0]
clpoint = closestPoint(gl, st, ob)
dist = gps_distance(clpoint[1], clpoint[0], ob[1], ob[0])
lat = clpoint[1] + (clpoint[1] - loc.lat) * (DistFromLine / dist)
lng = clpoint[0] + (clpoint[0] - loc.lng) * (DistFromLine / dist)
height = 50 + 125 * (dist / 75)
safeloc = Locationwp().Set(lat, lng, height, 16)
return safeloc
def searchGridSet():
# lst = [Locationwp().Set(,,), 16)]
lst = []
cam = Locationwp()
Locationwp.id.SetValue(cam, 206)
Locationwp.p1.SetValue(cam, 40.0)
# lst.append(cam)
wpfileLoc = 'Path_Planning/data/SearchGridWps.waypoints'
with open(wpfileLoc,"r") as globallist:
if (globallist.readline().split()[1] != "WPL"):
print "Nothing found"
else:
dat = globallist.readline().split()
dat = globallist.readline().split()
i = 1
while(len(dat) > 5):
if (dat[3] == '16'):
lst.append(Locationwp().Set(float(dat[8]),float(dat[9]),200, 16))
# print dat[10]
dat = globallist.readline().split(" ")
cam = Locationwp()
Locationwp.id.SetValue(cam, 206)
Locationwp.p1.SetValue(cam, 0)
# lst.append(cam)
searchGridPlan = [True,False,True,False,True,
False,True,False,True,False,
True,False,True,False,False]
# print lst
return lst
def offAxisSet(): dist = offAxisDist height = offAxisheight angle = atan(dist/height) bear = offAxisbearing LookRight = LookRight safeloc = [offAxisMeters[0]+LookRight*dist*cos(bear*pi/180), offAxisMeters[1]+LookRight*dist*sin(bear*pi/180), height] predist = -150 prex = safeloc[0] + predist*cos((bear+90)*pi/180) prey = safeloc[1] + predist*sin((bear+90)*pi/180) pre = cord_System.toGPS([prex,prey,height]) preloc = Locationwp().Set(pre[0],pre[1],height, 16) setangle = Locationwp() Locationwp.id.SetValue(setangle, 205) Locationwp.p2.SetValue(setangle, angle) offloc = cord_System.toGPS(safeloc) offloc = Locationwp().Set(offloc[0],offloc[1],height, 16) takephoto = Locationwp() Locationwp.id.SetValue(takephoto, 203) after1dist = 20 prex = safeloc[0] + after1dist*cos((bear+90)*pi/180) prey = safeloc[1] + after1dist*sin((bear+90)*pi/180) post1 = cord_System.toGPS([prex,prey,height]) post1 = Locationwp().Set(post1[0],post1[1],height, 16) resetangle = Locationwp() Locationwp.id.SetValue(resetangle, 205) Locationwp.p2.SetValue(resetangle, 0) postdist2 = 40 prex = safeloc[0] + postdist2*cos((bear+90)*pi/180) prey = safeloc[1] + postdist2*sin((bear+90)*pi/180) post2 = cord_System.toGPS([prex,prey,height]) postloc = Locationwp().Set(post2[0],post2[1],height, 16) # print 'Coordinates set with angle ',angle*180/3.1415, ' and distance ', (dist**2+height**2)**.5 return [preloc,setangle,offloc,post1,resetangle,postloc]#,cord_System.MetertoWp(offAxisMeters)]
def readWpFile(wpfilename):
globallist = open(wpfilename, "r")
lngth = file_len(wpfilename)
lst = []
if (globallist.readline().split(" ")[1] == "WPL"):
globallist.readline()
for wpnum in range(lngth - 2):
dat = globallist.readline().split(" ")
cam = Locationwp()
Locationwp.id.SetValue(cam, int(dat[3]))
Locationwp.p1.SetValue(cam, float(dat[4]))
Locationwp.p2.SetValue(cam, float(dat[5]))
Locationwp.p3.SetValue(cam, float(dat[6]))
Locationwp.p4.SetValue(cam, float(dat[7]))
Locationwp.lat.SetValue(cam, float(dat[8]))
Locationwp.lng.SetValue(cam, float(dat[9]))
Locationwp.alt.SetValue(cam, float(dat[10]))
lst.append(cam) #append cam trig
else:
print "Nothing found"
globallist.close()
return lst
def __init__(self, start):
self.dataPath = 'C:/Users/imaging2.0/Documents/MUAS-17/Flight_Path/flight_information.txt'
self.intPath = 'C:/Users/imaging2.0/Documents/MUAS-17/Flight_Path/intermediate_waypoints.txt'
self.BoundsLoc = 'C:/Users/imaging2.0/Documents/MUAS-17/Flight_Path/boundry.txt'
self.MovingObstacleLoc = 'C:/Users/imaging2.0/Documents/MUAS-17/Mission-Data/moving_obstacle_data.txt'
self.StaticObstacleLoc = 'C:/Users/imaging2.0/Documents/MUAS-17/Mission-Data/static_obstacle_data.txt'
self.Safety = 6
self.cruise = 16
if (start == None):
self.Home = Locationwp().Set(cs.lat, cs.lng, 0, 16)
else:
self.Home = start
self.Bounds = []
self.addBounds()
self.printBounds()
self.StaticObstacles = []
self.addStaticObstacles()
print "finished Dstar"
#dibuat oleh [email protected], id Line: wisnufireball #kalau ada yang mau ditanya, kontak aja. # Koordinat pake titik #RUN SCRIPT SETELAH TERBANG ### INISIALISASI ### print 'Mulai v0.1' import sys import clr import time clr.AddReference("MissionPlanner") import MissionPlanner clr.AddReference("MissionPlanner.Utilities") # includes the Utilities class from MissionPlanner.Utilities import Locationwp relay_target = Locationwp() #objek wp, ya bayangkan aja variabel tipe wp print 'Init Complete' while True: if ((Ports[1].MAV.cs.mode != "Loiter") & (Ports[1].MAV.cs.mode != "Stabilize")): Locationwp.lat.SetValue(relay_target, (Ports[0].MAV.cs.lat + -6.8868613) / 2) Locationwp.lng.SetValue(relay_target, (Ports[0].MAV.cs.lng + 107.608548) / 2) Locationwp.alt.SetValue(relay_target, 10) Ports[1].setGuidedModeWP(relay_target) print 'Relay Target Updated' Script.Sleep(5000) print 'Script Selesai' print 'wisnu emang ganteng'
def payloaddropSet(): bear = dropbearing + pi height = 115.0 windbearing = 0 windoffset = 0 dist = 110.0 #function of alt and vel distprevious = 250.0 distafter = -120.0 prey = dropMeters[1] + (distprevious+dist)*sin(bear) prex = dropMeters[0] + (distprevious+dist)*cos(bear) pre = [prex,prey,height] cr.Waypoint(16,[]) preGPS = cord_System.toGPS(pre) pre = Locationwp().Set(preGPS[0],preGPS[1],preGPS[2], 16) posty = dropMeters[1] + (distafter+dist)*sin(bear) postx = dropMeters[0] + (distafter+dist)*cos(bear) post = [postx,posty,height] postGPS = cord_System.toGPS(post) post = Locationwp().Set(postGPS[0],postGPS[1],postGPS[2], 16) dropy = dropMeters[1] + dist*sin(bear) dropx = dropMeters[0] + dist*cos(bear) drop = [dropx,dropy,height] dropGPS = cord_System.toGPS(drop) drop = Locationwp().Set(dropGPS[0],dropGPS[1],dropGPS[2], 16) dropWP1 = Locationwp() Locationwp.id.SetValue(dropWP1, 183) Locationwp.p1.SetValue(dropWP1, 10) # servo number Locationwp.p2.SetValue(dropWP1, 1900) # ms dropWP2 = Locationwp() Locationwp.id.SetValue(dropWP2, 183) Locationwp.p1.SetValue(dropWP2, 11) # servo number Locationwp.p2.SetValue(dropWP2, 1100) # ms OpenWP1 = Locationwp() Locationwp.id.SetValue(OpenWP1, 183) Locationwp.p1.SetValue(OpenWP1, 8) Locationwp.p2.SetValue(OpenWP1, 950) OpenWP2 = Locationwp() Locationwp.id.SetValue(OpenWP2, 183) Locationwp.p1.SetValue(OpenWP2, 9) Locationwp.p2.SetValue(OpenWP2, 2100) CloseWP1 = Locationwp() Locationwp.id.SetValue(CloseWP1, 183) Locationwp.p1.SetValue(CloseWP1, 8) Locationwp.p2.SetValue(CloseWP1, 2100) CloseWP2 = Locationwp() Locationwp.id.SetValue(CloseWP2, 183) Locationwp.p1.SetValue(CloseWP2, 9) Locationwp.p2.SetValue(CloseWP2, 900) return [pre, OpenWP1, OpenWP2, drop, dropWP1, dropWP2, post, CloseWP2, CloseWP2]
import clr
import time
import System
from System import Byte
clr.AddReference("MissionPlanner")
import MissionPlanner
clr.AddReference("MissionPlanner.Utilities") # includes the Utilities class
from MissionPlanner.Utilities import Locationwp
clr.AddReference("MAVLink") # includes the Utilities class
import MAVLink
idmavcmd = MAVLink.MAV_CMD.WAYPOINT
id = int(idmavcmd)
home = Locationwp().Set(-34.9805,117.8518,0, id)
to = Locationwp()
Locationwp.id.SetValue(to, int(MAVLink.MAV_CMD.TAKEOFF))
Locationwp.p1.SetValue(to, 15)
Locationwp.alt.SetValue(to, 50)
wp1 = Locationwp().Set(-35,117.8,50, id)
wp2 = Locationwp().Set(-35,117.89,50, id)
wp3 = Locationwp().Set(-35,117.85,20, id)
print "set wp total"
MAV.setWPTotal(5)
print "upload home - reset on arm"
MAV.setWP(home,0,MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT);
print "upload to"
MAV.setWP(to,1,MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT);
print "upload wp1"
def toGPS(self, loc):
dlat = (loc[0]) * delLat + self.Home.lat
dlng = (loc[1]) * delLng + self.Home.lng
alt = loc[2]
return Locationwp().Set(dlat, dlng, alt, 16)
def takeoff():
to = Locationwp()
Locationwp.id.SetValue(to, int(MAVLink.MAV_CMD.TAKEOFF))
Locationwp.p1.SetValue(to, 15)
Locationwp.alt.SetValue(to, 40)
return [to]
360) * descent_dist * deglatpermeter
lng_delta1 = math.sin(approach_azimuth * 2 * math.pi /
360) * descent_dist * deglngpermeter
lat1 = lathome + lat_delta1 + lat_delta2
lng1 = lnghome + lng_delta1 + lng_delta2
lat_delta3 = math.cos((approach_azimuth + 180) * 2 * math.pi /
360) * fly_thru_dist * deglatpermeter
lng_delta3 = math.sin((approach_azimuth + 180) * 2 * math.pi /
360) * fly_thru_dist * deglngpermeter
lat3 = lat2 + lat_delta3
lng3 = lng2 + lng_delta3
wp1 = Locationwp().Set(lathome, lnghome, current_alt, id)
wp2 = Locationwp().Set(lat1, lng1, alt_mid, id)
wp3 = Locationwp().Set(lat2, lng2, parachute_deploy_altitude, id)
wp4 = Locationwp().Set(lat3, lng3, parachute_deploy_altitude, id)
MAV.setWPTotal(5) #set wp total
MAV.setWP(MAV.getWP(0), 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
#upload home - reset on arm
MAV.setWP(wp1, 1, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
#upload wp1
MAV.setWP(wp2, 2, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
#upload wp2
MAV.setWP(wp3, 3, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
#upload wp3
MAV.setWP(wp4, 4, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
#upload wp4
def offaxistarget(loc, normal):
boundry1, boundry2 = findclosestboundries(loc, bounds)
safeloc = findSafeOffAxisLoc(boundry1, boundry2, loc)
#negative 1 if want to go RtoL
LtoR = OffAxisLtoR
dist = (((safeloc.lat - loc.lat) / delLat)**2 +
((safeloc.lng - loc.lng) / delLng)**2)**.5
#height = safeloc.alt
height = 100
dogleg = True
angle = atan(dist / height)
print 'angle at ', angle, ' degrees'
dlat = (loc.lat - safeloc.lat) / delLat
dlng = (loc.lng - safeloc.lng) / delLng
phi = atan2(dlat, dlng)
direc = phi + .5 * pi * LtoR
if (normal):
if (dogleg):
predist0 = -206
latPre0 = safeloc.lat + (predist0 * sin(direc -
(14 * pi / 180))) * delLat
lngPre0 = safeloc.lng + (predist0 * cos(direc -
(14 * pi / 180))) * delLng
preOff0 = Locationwp().Set(latPre0, lngPre0, height, 16)
else:
predist0 = -206
latPre0 = safeloc.lat + (predist0 * sin(direc)) * delLat
lngPre0 = safeloc.lng + (predist0 * cos(direc)) * delLng
preOff0 = Locationwp().Set(latPre0, lngPre0, height, 16)
predist1 = -165
latPre1 = safeloc.lat + (predist1 * sin(direc)) * delLat
lngPre1 = safeloc.lng + (predist1 * cos(direc)) * delLng
preOff1 = Locationwp().Set(latPre1, lngPre1, height, 16)
setangle = Locationwp()
Locationwp.id.SetValue(setangle, 205)
Locationwp.p2.SetValue(setangle, angle)
print 'half'
predist2 = -50
latPre2 = safeloc.lat + (predist2 * sin(direc)) * delLat
lngPre2 = safeloc.lng + (predist2 * cos(direc)) * delLng
preOff2 = Locationwp().Set(latPre2, lngPre2, height, 16)
takephoto = Locationwp()
Locationwp.id.SetValue(takephoto, 203)
afterDist = 40
lataft = safeloc.lat + (afterDist * sin(direc)) * delLat
lngaft = safeloc.lng + (afterDist * cos(direc)) * delLng
afterOff = Locationwp().Set(lataft, lngaft, height, 16)
resetangle = Locationwp()
Locationwp.id.SetValue(resetangle, 205)
Locationwp.p2.SetValue(resetangle, 0)
return [
preOff0, preOff1, preOff2, setangle, safeloc, takephoto, afterOff,
resetangle
]
else:
predist2 = -50
latPre2 = safeloc.lat + (predist2 * sin(direc)) * delLat
lngPre2 = safeloc.lng + (predist2 * cos(direc)) * delLng
preOff2 = Locationwp().Set(latPre2, lngPre2, height, 16)
setangle = Locationwp()
Locationwp.id.SetValue(setangle, 205)
Locationwp.p2.SetValue(setangle, angle)
loiterturns = Locationwp().Set(safeloc.lat, safeloc.lng, height, 18)
Locationwp.p1.SetValue(loiterturns, 5)
Locationwp.id.SetValue(loiterturns, 50)
afterDist = 40
lataft = safeloc.lat + (afterDist * sin(direc)) * delLat
lngaft = safeloc.lng + (afterDist * cos(direc)) * delLng
afterOff = Locationwp().Set(lataft, lngaft, height, 16)
return [preoff, setangle, loiterturns, afterDist]
import MissionPlanner
clr.AddReference("MissionPlanner.Utilities") #includes the Utilities class
from MissionPlanner.Utilities import Locationwp
clr.AddReference("MAVLink") # includes the Utilities class
import MAVLink
#SPEAKING
#need to enable this
MissionPlanner.MainV2.speechEnable = True
#call this with the string you want it to say
MissionPlanner.MainV2.speechEngine.SpeakAsync(string)
#WAYPOINT OBJECTS
Locationwp().Set(latitude,longitude,altitude, 16)
'''
Waypoint type IDs
16 Waypoint
21 land
82 Spline Waypoint
18 Loiter Turns
19 Loiter Turns
17 Loiter Unlim
20 Return to Launch
22 takeoff
93 delay
the rest can be found through the following line but change TAKEOFF to the type of waypoint as found in mission planner
'''
int(MAVLink.MAV_CMD.TAKEOFF)
turn1_azimuth * 2 * math.pi / 360) * turn_dist * deglatpermeter
lng_delta2 = math.sin(
turn1_azimuth * 2 * math.pi / 360) * turn_dist * deglngpermeter
lat2 = lat1 + lat_delta2
lng2 = lng1 + lng_delta2
lat_delta3 = math.cos(
turn2_azimuth * 2 * math.pi / 360) * turn_dist * deglatpermeter
lng_delta3 = math.sin(
turn2_azimuth * 2 * math.pi / 360) * turn_dist * deglngpermeter
lat3 = lat2 + lat_delta3
lng3 = lng2 + lng_delta3
takeoff = Locationwp()
Locationwp.id.SetValue(takeoff, int(MAVLink.MAV_CMD.TAKEOFF))
Locationwp.p1.SetValue(takeoff, pitch)
Locationwp.alt.SetValue(takeoff, launch_altitude)
wp1 = Locationwp().Set(lat1, lng1, alt_mid, id)
wp2 = Locationwp().Set(lat2, lng2, alt_mid, id)
wp3 = Locationwp().Set(lat3, lng3, alt_mid, id)
wp4 = Locationwp().Set(lathome, lnghome, cruise_altitude, id)
MAV.setWPTotal(6) #set wp total
MAV.setWP(MAV.getWP(0), 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
#upload home - reset on arm
MAV.setWP(takeoff, 1, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
#upload takeoff
MAV.setWP(wp1, 2, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
#upload wp1
MAV.setWP(wp2, 3, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT)
def set_waypoint(self, coordinate):
wp1 = Locationwp().Set(coordinate.latitude, coordinate.longitude,
coordinate.altitude,
int(self.mavlink.MAV_CMD.WAYPOINT))
self.mav.setGuidedModeWP(wp1, True)
