def scan(self, filename):
'''scan a tlog file for positions'''
mlog = mavutil.mavlink_connection(filename, robust_parsing=True)
idx = 0
print("Scanning %u images" % len(self.images))
while idx < len(self.images):
msg = mlog.recv_match(type='GLOBAL_POSITION_INT')
if msg is None:
break
bearing = msg.hdg * 0.01
speed = math.sqrt(msg.vx**2 + msg.vy**2)
while idx < len(self.images
) and msg._timestamp > self.images[idx].frame_time:
dt = msg._timestamp - self.images[idx].frame_time
pos = (msg.lat * 1.0e-7, msg.lon * 1.0e-7)
pos = cuav_util.gps_newpos(pos[0], pos[1], bearing, speed * dt)
self.images[idx].pos = pos
latint = int(pos[0] * 1000)
lonint = int(pos[1] * 1000)
if not latint in self.latset:
self.latset[latint] = set()
if not lonint in self.lonset:
self.lonset[lonint] = set()
self.latset[latint].add(idx)
self.lonset[lonint].add(idx)
idx += 1
print("Scanned %u images" % idx)
def scan(self, filename):
'''scan a tlog file for positions'''
mlog = mavutil.mavlink_connection(filename, robust_parsing=True)
idx = 0
print("Scanning %u images" % len(self.images))
while idx < len(self.images):
msg = mlog.recv_match(type='GLOBAL_POSITION_INT')
if msg is None:
break
bearing = msg.hdg*0.01
speed = math.sqrt(msg.vx**2 + msg.vy**2)
while idx < len(self.images) and msg._timestamp > self.images[idx].frame_time:
dt = msg._timestamp - self.images[idx].frame_time
pos = (msg.lat*1.0e-7, msg.lon*1.0e-7)
pos = cuav_util.gps_newpos(pos[0], pos[1], bearing, speed*dt)
self.images[idx].pos = pos
latint = int(pos[0]*1000)
lonint = int(pos[1]*1000)
if not latint in self.latset:
self.latset[latint] = set()
if not lonint in self.lonset:
self.lonset[lonint] = set()
self.latset[latint].add(idx)
self.lonset[lonint].add(idx)
idx += 1
print("Scanned %u images" % idx)
def position(self, t, max_deltat=0,roll=None, pitch=None, maxroll=0, maxpitch=0, pitch_offset=0, roll_offset=0):
'''return a MavPosition estimate given a time'''
self.advance_log(t)
# extrapolate our latitude/longitude
gpst = t + self.gps_lag
gps_raw = self._find_msg('GLOBAL_POSITION_INT', gpst)
gps_timestamp = gps_raw._timestamp
velocity = math.sqrt((gps_raw.vx*0.01)**2 + (gps_raw.vy*0.01)**2)
deltat = gpst - gps_timestamp
(lat, lon) = cuav_util.gps_newpos(gps_raw.lat/1.0e7, gps_raw.lon/1.0e7,
gps_raw.hdg*0.01,
velocity * (gpst - gps_timestamp))
scaled_pressure = self._find_msg('SCALED_PRESSURE', t)
terrain_report = None
if len(self.terrain_report) > 0:
terrain_report = self._find_msg('TERRAIN_REPORT', t)
if terrain_report is not None:
(tlat, tlon) = (terrain_report.lat/1.0e7, terrain_report.lon/1.0e7)
# don't use it if its too far away
if (cuav_util.gps_distance(lat, lon, tlat, tlon) > 150 or
abs(terrain_report._timestamp - t) > 5):
terrain_report = None
# get altitude
altitude = self._altitude(scaled_pressure, terrain_report)
# and attitude
if roll is None:
roll = math.degrees(self.interpolate_angle('ATTITUDE', 'roll', t, max_deltat))
if abs(roll) < maxroll:
# camera stabilisation system can take care of it
roll = 0
elif roll >= maxroll:
# adjust for roll stabilisation system can't handle
roll = roll - maxroll
else:
# adjust for roll stabilisation system can't handle
roll = roll + maxroll
if pitch is None:
pitch = math.degrees(self.interpolate_angle('ATTITUDE', 'pitch', t, max_deltat))
if abs(pitch) < maxpitch:
pitch = 0
elif pitch >= maxpitch:
pitch = pitch - maxpitch
else:
pitch = pitch + maxpitch
# add pitch and roll offset
pitch += pitch_offset
roll += roll_offset
yaw = math.degrees(self.interpolate_angle('ATTITUDE', 'yaw', t, max_deltat))
return MavPosition(lat, lon, altitude, roll, pitch, yaw, t)
def position(self, t, max_deltat=0, roll=None, maxroll=45):
'''return a MavPosition estimate given a time'''
self.advance_log(t)
scaled_pressure = self._find_msg('SCALED_PRESSURE', t)
# extrapolate our latitude/longitude
gpst = t + self.gps_lag
if mavutil.mavlink10():
gps_raw = self._find_msg('GPS_RAW_INT', gpst)
gps_timestamp = self.gps_time(gps_raw)
#print gps_raw._timestamp, gps_timestamp, gpst, t, gpst - gps_timestamp
(lat, lon) = cuav_util.gps_newpos(
gps_raw.lat / 1.0e7, gps_raw.lon / 1.0e7, gps_raw.cog * 0.01,
(gps_raw.vel * 0.01) * (gpst - gps_timestamp))
else:
gps_raw = self._find_msg('GPS_RAW', gpst)
gps_timestamp = self.gps_time(gps_raw)
(lat,
lon) = cuav_util.gps_newpos(gps_raw.lat, gps_raw.lon, gps_raw.hdg,
gps_raw.v * (gpst - gps_timestamp))
# get altitude
altitude = self._altitude(scaled_pressure)
# and attitude
mavroll = math.degrees(
self.interpolate_angle('ATTITUDE', 'roll', t, max_deltat))
if roll is None:
roll = mavroll
elif abs(mavroll) < maxroll:
roll = 0
elif mavroll >= maxroll:
roll = mavroll - maxroll
else:
roll = mavroll + maxroll
pitch = math.degrees(
self.interpolate_angle('ATTITUDE', 'pitch', t, max_deltat))
yaw = math.degrees(
self.interpolate_angle('ATTITUDE', 'yaw', t, max_deltat))
return MavPosition(lat, lon, altitude, roll, pitch, yaw, t)
def position(self, t, max_deltat=0, roll=None, maxroll=45):
"""return a MavPosition estimate given a time"""
self.advance_log(t)
scaled_pressure = self._find_msg("SCALED_PRESSURE", t)
# extrapolate our latitude/longitude
gpst = t + self.gps_lag
if mavutil.mavlink10():
gps_raw = self._find_msg("GPS_RAW_INT", gpst)
gps_timestamp = self.gps_time(gps_raw)
# print gps_raw._timestamp, gps_timestamp, gpst, t, gpst - gps_timestamp
(lat, lon) = cuav_util.gps_newpos(
gps_raw.lat / 1.0e7,
gps_raw.lon / 1.0e7,
gps_raw.cog * 0.01,
(gps_raw.vel * 0.01) * (gpst - gps_timestamp),
)
else:
gps_raw = self._find_msg("GPS_RAW", gpst)
gps_timestamp = self.gps_time(gps_raw)
(lat, lon) = cuav_util.gps_newpos(gps_raw.lat, gps_raw.lon, gps_raw.hdg, gps_raw.v * (gpst - gps_timestamp))
# get altitude
altitude = self._altitude(scaled_pressure)
# and attitude
mavroll = math.degrees(self.interpolate_angle("ATTITUDE", "roll", t, max_deltat))
if roll is None:
roll = mavroll
elif abs(mavroll) < maxroll:
roll = 0
elif mavroll >= maxroll:
roll = mavroll - maxroll
else:
roll = mavroll + maxroll
pitch = math.degrees(self.interpolate_angle("ATTITUDE", "pitch", t, max_deltat))
yaw = math.degrees(self.interpolate_angle("ATTITUDE", "yaw", t, max_deltat))
return MavPosition(lat, lon, altitude, roll, pitch, yaw, t)
def position(self, t, max_deltat=0, roll=None, maxroll=45):
'''return a MavPosition estimate given a time'''
self.advance_log(t)
try:
# extrapolate our latitude/longitude
gpst = t + self.gps_lag
gps_raw = self._find_msg('GLOBAL_POSITION_INT', gpst)
gps_timestamp = gps_raw._timestamp
velocity = math.sqrt((gps_raw.vx * 0.01)**2 +
(gps_raw.vy * 0.01)**2)
deltat = gpst - gps_timestamp
(lat,
lon) = cuav_util.gps_newpos(gps_raw.lat / 1.0e7,
gps_raw.lon / 1.0e7,
gps_raw.hdg * 0.01,
velocity * (gpst - gps_timestamp))
scaled_pressure = self._find_msg('SCALED_PRESSURE', t)
terrain_report = None
if len(self.terrain_report) > 0:
terrain_report = self._find_msg('TERRAIN_REPORT', t)
if terrain_report is not None:
(tlat, tlon) = (terrain_report.lat / 1.0e7,
terrain_report.lon / 1.0e7)
# don't use it if its too far away
if (cuav_util.gps_distance(lat, lon, tlat, tlon) > 150
or abs(terrain_report._timestamp - t) > 5):
terrain_report = None
# get altitude
altitude = self._altitude(scaled_pressure, terrain_report)
# and attitude
mavroll = math.degrees(
self.interpolate_angle('ATTITUDE', 'roll', t, max_deltat))
if roll is None:
roll = mavroll
elif abs(mavroll) < maxroll:
roll = 0
elif mavroll >= maxroll:
roll = mavroll - maxroll
else:
roll = mavroll + maxroll
pitch = math.degrees(
self.interpolate_angle('ATTITUDE', 'pitch', t, max_deltat))
yaw = math.degrees(
self.interpolate_angle('ATTITUDE', 'yaw', t, max_deltat))
return MavPosition(lat, lon, altitude, roll, pitch, yaw, t)
except Exception as exception:
return MavPosition(0.0, 0.0, 0, 0, 0, 0, t)
def get_ground_alt(lat, lon):
"""get highest ground altitide around a point"""
global EleModel
ground = EleModel.GetElevation(lat, lon)
surrounds = []
for bearing in range(0, 360, 45):
surrounds.append((opts.lookahead, bearing))
for (dist, bearing) in surrounds:
(lat2, lon2) = cuav_util.gps_newpos(lat, lon, bearing, dist)
el = EleModel.GetElevation(lat2, lon2)
if el > ground:
ground = el
return ground
def get_ground_alt(lat, lon):
'''get highest ground altitide around a point'''
global EleModel
ground = EleModel.GetElevation(lat, lon)
surrounds = []
for bearing in range(0, 360, 45):
surrounds.append((opts.lookahead, bearing))
for (dist, bearing) in surrounds:
(lat2, lon2) = cuav_util.gps_newpos(lat, lon, bearing, dist)
el = EleModel.GetElevation(lat2, lon2)
if el > ground:
ground = el
return ground
def add_points(wp, argagl, argstep, argmaxdelta, argrtlalt):
'''add more points for terrain following'''
wplist = []
wplist2 = []
for i in range(0, wp.count()):
wplist.append(wp.wp(i))
wplist[0].z = argagl
# add in RTL
wplist.append(wplist[0])
wplist2.append(wplist[0])
home = wp.wp(0)
home_ground = EleModel.GetElevation(home.x, home.y)
for i in range(1, len(wplist)):
prev = (wplist2[-1].x, wplist2[-1].y, wplist2[-1].z)
dist = cuav_util.gps_distance(wplist2[-1].x, wplist2[-1].y, wplist[i].x, wplist[i].y)
bearing = cuav_util.gps_bearing(wplist2[-1].x, wplist2[-1].y, wplist[i].x, wplist[i].y)
print("dist=%u bearing=%u" % (dist, bearing))
while dist > argstep:
newpos = cuav_util.gps_newpos(prev[0], prev[1], bearing, argstep)
ground1 = EleModel.GetElevation(prev[0], prev[1])
ground2 = EleModel.GetElevation(newpos[0], newpos[1])
if ground2 == None:
ground2 = home_ground
agl = (home_ground + prev[2]) - ground2
if abs(agl - argagl) > argmaxdelta:
newwp = copy.copy(wplist2[-1])
newwp.x = newpos[0]
newwp.y = newpos[1]
newwp.z = (ground2 + argagl) - home_ground
wplist2.append(newwp)
print("Inserting at %u" % newwp.z)
prev = (newpos[0], newpos[1], newwp.z)
else:
prev = (newpos[0], newpos[1], wplist2[-1].z)
dist -= argstep
wplist2.append(wplist[i])
wplist2[-1].z = argrtlalt
wp2 = mavwp.MAVWPLoader()
for w in wplist2:
wp2.add(w)
wp2.save("newwp.txt")
return wp2
def position(self, t, max_deltat=0,roll=None, maxroll=45):
'''return a MavPosition estimate given a time'''
self.advance_log(t)
try:
# extrapolate our latitude/longitude
gpst = t + self.gps_lag
gps_raw = self._find_msg('GLOBAL_POSITION_INT', gpst)
gps_timestamp = gps_raw._timestamp
velocity = math.sqrt((gps_raw.vx*0.01)**2 + (gps_raw.vy*0.01)**2)
deltat = gpst - gps_timestamp
(lat, lon) = cuav_util.gps_newpos(gps_raw.lat/1.0e7, gps_raw.lon/1.0e7,
gps_raw.hdg*0.01,
velocity * (gpst - gps_timestamp))
scaled_pressure = self._find_msg('SCALED_PRESSURE', t)
terrain_report = None
if len(self.terrain_report) > 0:
terrain_report = self._find_msg('TERRAIN_REPORT', t)
if terrain_report is not None:
(tlat, tlon) = (terrain_report.lat/1.0e7, terrain_report.lon/1.0e7)
# don't use it if its too far away
if (cuav_util.gps_distance(lat, lon, tlat, tlon) > 150 or
abs(terrain_report._timestamp - t) > 5):
terrain_report = None
# get altitude
altitude = self._altitude(scaled_pressure, terrain_report)
# and attitude
mavroll = math.degrees(self.interpolate_angle('ATTITUDE', 'roll', t, max_deltat))
if roll is None:
roll = mavroll
elif abs(mavroll) < maxroll:
roll = 0
elif mavroll >= maxroll:
roll = mavroll - maxroll
else:
roll = mavroll + maxroll
pitch = math.degrees(self.interpolate_angle('ATTITUDE', 'pitch', t, max_deltat))
yaw = math.degrees(self.interpolate_angle('ATTITUDE', 'yaw', t, max_deltat))
return MavPosition(lat, lon, altitude, roll, pitch, yaw, t)
except Exception as exception:
return MavPosition(0.0, 0.0, 0, 0, 0, 0,t)
def add_points(wp):
'''add more points for terrain following'''
wplist = []
wplist2 = []
for i in range(0, wp.count()):
wplist.append(wp.wp(i))
wplist[0].z = opts.agl
# add in RTL
wplist.append(wplist[0])
wplist2.append(wplist[0])
home = wp.wp(0)
home_ground = get_ground_alt(home.x, home.y)
for i in range(1, len(wplist)):
prev = (wplist2[-1].x, wplist2[-1].y, wplist2[-1].z)
dist = cuav_util.gps_distance(wplist2[-1].x, wplist2[-1].y,
wplist[i].x, wplist[i].y)
bearing = cuav_util.gps_bearing(wplist2[-1].x, wplist2[-1].y,
wplist[i].x, wplist[i].y)
print("dist=%u bearing=%u" % (dist, bearing))
while dist > opts.step:
newpos = cuav_util.gps_newpos(prev[0], prev[1], bearing, opts.step)
ground1 = get_ground_alt(prev[0], prev[1])
ground2 = get_ground_alt(newpos[0], newpos[1])
agl = (home_ground + prev[2]) - ground2
if abs(agl - opts.agl) > opts.maxdelta:
newwp = copy.copy(wplist2[-1])
newwp.x = newpos[0]
newwp.y = newpos[1]
newwp.z = (ground2 + opts.agl) - home_ground
wplist2.append(newwp)
print("Inserting at %u" % newwp.z)
prev = (newpos[0], newpos[1], newwp.z)
else:
prev = (newpos[0], newpos[1], wplist2[-1].z)
dist -= opts.step
wplist2.append(wplist[i])
wplist2[-1].z = opts.rtlalt
wp2 = mavwp.MAVWPLoader()
for w in wplist2:
wp2.add(w)
wp2.save("newwp.txt")
return wp2
def add_points(wp):
"""add more points for terrain following"""
wplist = []
wplist2 = []
for i in range(0, wp.count()):
wplist.append(wp.wp(i))
wplist[0].z = opts.agl
# add in RTL
wplist.append(wplist[0])
wplist2.append(wplist[0])
home = wp.wp(0)
home_ground = get_ground_alt(home.x, home.y)
for i in range(1, len(wplist)):
prev = (wplist2[-1].x, wplist2[-1].y, wplist2[-1].z)
dist = cuav_util.gps_distance(wplist2[-1].x, wplist2[-1].y, wplist[i].x, wplist[i].y)
bearing = cuav_util.gps_bearing(wplist2[-1].x, wplist2[-1].y, wplist[i].x, wplist[i].y)
print("dist=%u bearing=%u" % (dist, bearing))
while dist > opts.step:
newpos = cuav_util.gps_newpos(prev[0], prev[1], bearing, opts.step)
ground1 = get_ground_alt(prev[0], prev[1])
ground2 = get_ground_alt(newpos[0], newpos[1])
agl = (home_ground + prev[2]) - ground2
if abs(agl - opts.agl) > opts.maxdelta:
newwp = copy.copy(wplist2[-1])
newwp.x = newpos[0]
newwp.y = newpos[1]
newwp.z = (ground2 + opts.agl) - home_ground
wplist2.append(newwp)
print("Inserting at %u" % newwp.z)
prev = (newpos[0], newpos[1], newwp.z)
else:
prev = (newpos[0], newpos[1], wplist2[-1].z)
dist -= opts.step
wplist2.append(wplist[i])
wplist2[-1].z = opts.rtlalt
wp2 = mavwp.MAVWPLoader()
for w in wplist2:
wp2.add(w)
wp2.save("newwp.txt")
return wp2
def altitudeCompensation(self, heightAGL, numMaxPoints=100, threshold=5):
'''Creates height points (ASL) for each point in searchArea,
entry and exit points such that the plane stays a constant altitude above the ground,
constrained by a max number of waypoints'''
maxDeltaAlt = 0
maxDeltaAltPoints = []
maxDeltapercentAlong = 0
EleModel = mp_elevation.ElevationModel()
#make sure the SRTM tiles are downloaded
EleModel.GetElevation(self.SearchPattern[0][0],
self.SearchPattern[0][1])
#get the ASL height of the airfield home, entry and exit points and initial search pattern
# and add the heightAGL to them
self.airportHeight = EleModel.GetElevation(self.airfieldHome[0],
self.airfieldHome[1])
if abs(opts.basealt - self.airportHeight) > 30:
print("BAD BASE ALTITUDE %u - airfieldhome %u" %
(opts.basealt, self.airportHeight))
sys.exit(1)
self.airportHeight = opts.basealt
self.airfieldHome = (self.airfieldHome[0], self.airfieldHome[1],
heightAGL + opts.basealt)
for point in self.entryPoints:
self.entryPoints[self.entryPoints.index(point)] = (
point[0], point[1],
heightAGL + 10 + EleModel.GetElevation(point[0], point[1]))
for point in self.exitPoints:
self.exitPoints[self.exitPoints.index(point)] = (
point[0], point[1],
heightAGL + 10 + EleModel.GetElevation(point[0], point[1]))
for point in self.SearchPattern:
self.SearchPattern[self.SearchPattern.index(point)] = (
point[0], point[1],
heightAGL + EleModel.GetElevation(point[0], point[1]))
#keep looping through the search area waypoints and add new waypoints where needed
#to maintain const height above terrain
print "---Starting terrain tracking optimisation---"
while True:
maxDeltaAlt = 0
maxDeltaAltPoints = []
maxDeltaPointIndex = 0
for point in self.SearchPattern:
if point != self.SearchPattern[-1]:
dist = cuav_util.gps_distance(
point[0], point[1],
self.SearchPattern[self.SearchPattern.index(point) +
1][0],
self.SearchPattern[self.SearchPattern.index(point) +
1][1])
theta = cuav_util.gps_bearing(
point[0], point[1],
self.SearchPattern[self.SearchPattern.index(point) +
1][0],
self.SearchPattern[self.SearchPattern.index(point) +
1][1])
AltDiff = float(self.SearchPattern[
self.SearchPattern.index(point) + 1][2] - point[2])
if numpy.around(theta) == numpy.around(
self.searchBearing) or numpy.around(
theta) == numpy.around(
(self.searchBearing + 180) % 360):
#increment 10% along waypoint-to-waypoint and get max height difference
for i in range(1, 9):
partPoint = cuav_util.gps_newpos(
point[0], point[1], theta, (i * dist / 10))
partAlt = EleModel.GetElevation(
partPoint[0], partPoint[1]) + heightAGL
#print "Part = " + str(partAlt) + ", Orig = " + str(point[2])
if numpy.abs(point[2] + (
(AltDiff * i) / 10) - partAlt) > maxDeltaAlt:
maxDeltaAlt = numpy.abs(point[2] +
((AltDiff * i) / 10) -
partAlt)
maxDeltaAltPoint = (partPoint[0], partPoint[1],
partAlt)
maxDeltaPointIndex = self.SearchPattern.index(
point) + 1
#print "New max alt diff: " + str(maxDeltaAlt) + ", " + str(partAlt)
#now put a extra waypoint in between the two maxDeltaAltPoints
if maxDeltaAltPoint is not None:
self.SearchPattern.insert(maxDeltaPointIndex, maxDeltaAltPoint)
#print "There are " + str(len(self.SearchPattern)) + " points in the search pattern. Max Alt error is " + str(maxDeltaAlt)
if len(self.SearchPattern
) >= numMaxPoints or threshold > maxDeltaAlt:
break
print "---Done terrain tracking optimisation---"
def exportToMAVProxy(self, loiterInSearchArea=1):
'''Exports the airfield home, entry points, search pattern and exit points to MAVProxy'''
#make a fake waypoint loader for testing purposes, if we're not
#running within MAVProxy
if self.MAVpointLoader is None:
print "No loader - creating one"
self.MAVpointLoader = mavwp.MAVWPLoader()
MAVpointLoader = self.MAVpointLoader
entryjump = []
exitjump = []
#clear out all the old waypoints
MAVpointLoader.clear()
TargetSys = MAVpointLoader.target_system
TargetComp = MAVpointLoader.target_component
#WP0 - add landingPt as waypoint 0. This gets replaced when GPS gets lock
w = self.waypoint(*self.landingPt, alt=opts.basealt,
frame=mavutil.mavlink.MAV_FRAME_GLOBAL)
MAVpointLoader.add(w, comment='Airfield home')
#WP2 - takeoff, then jump to entry lanes
w = self.waypoint(*cuav_util.gps_newpos(*self.landingPt, bearing=self.landingHeading+175, distance=300), alt=40,
cmd=mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, param=[12, 0, 0, 0])
MAVpointLoader.add(w, comment="Takeoff")
entryjump.append(MAVpointLoader.count())
w = self.jump(0)
MAVpointLoader.add(w, comment='Jump to entry lane')
# landing approach 1
landing_approach_wpnum = MAVpointLoader.count()
w = self.waypoint(*cuav_util.gps_newpos(*self.landingPt, bearing=self.landingHeading+180, distance=1000), alt=80)
MAVpointLoader.add(w, comment='Landing approach')
# drop our speed
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, param=[0, 23, 20, 0])
MAVpointLoader.add(w, comment='Change to 23 m/s')
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, param=[1, 23, 20, 0])
MAVpointLoader.add(w, comment='Change throttle to 20%%')
# landing approach 1
landing_approach_wpnum = MAVpointLoader.count()
w = self.waypoint(*cuav_util.gps_newpos(*self.landingPt, bearing=self.landingHeading+180, distance=500), alt=50)
MAVpointLoader.add(w, comment='Landing approach 2')
# landing
w = self.waypoint(self.landingPt[0], self.landingPt[1], 0, cmd=mavutil.mavlink.MAV_CMD_NAV_LAND)
MAVpointLoader.add(w, comment='Landing')
# comms Failure. Loiter at EL-1 for 2 minutes then fly to airfield home and loiter
point = self.entryPoints[0]
w = self.waypoint(point[0], point[1], self.airportHeight)
MAVpointLoader.add(w, comment='Comms Failure')
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, param=[120, 0, 0, 0])
MAVpointLoader.add(w, comment='loiter 2 minutes')
w = self.waypoint(self.airfieldHome[0], self.airfieldHome[1], self.airportHeight)
MAVpointLoader.add(w, comment='Airfield home')
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM)
MAVpointLoader.add(w, comment='loiter')
# GPS failure. Loiter in place for 30s then direct to airfield home
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME, param=[30,0,0,0])
MAVpointLoader.add(w, comment='GPS fail - loiter 30 secs')
w = self.waypoint(self.airfieldHome[0], self.airfieldHome[1], self.airportHeight)
MAVpointLoader.add(w, comment='Airfield home')
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM)
MAVpointLoader.add(w, comment='loiter')
dropalt = self.joePos[2]
joe = (self.joePos[0], self.joePos[1])
crosswp = MAVpointLoader.count()
w = self.waypoint(*cuav_util.gps_newpos(*joe, bearing=60, distance=250), alt=dropalt)
MAVpointLoader.add(w, comment='cross 1')
w = self.waypoint(*cuav_util.gps_newpos(*joe, bearing=240, distance=250), alt=dropalt)
MAVpointLoader.add(w, comment='cross 2')
w = self.waypoint(*cuav_util.gps_newpos(*joe, bearing=300, distance=250), alt=dropalt)
MAVpointLoader.add(w, comment='cross 3')
w = self.waypoint(*cuav_util.gps_newpos(*joe, bearing=120, distance=250), alt=dropalt)
MAVpointLoader.add(w, comment='cross 4')
w = self.jump(crosswp)
MAVpointLoader.add(w, comment='Jump to cross')
# joe approach point
w = self.waypoint(*cuav_util.gps_newpos(*joe, bearing=0, distance=350), alt=dropalt)
MAVpointLoader.add(w, comment='Joe approach')
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, param=[0, 20, 50, 0])
MAVpointLoader.add(w, comment='Change to 20 m/s')
# joe location. We use a default acceptance radius of 35 meters. Will be adjusted with 'wp param'
# command
w = self.waypoint(*joe, alt=dropalt, param=[0, 35, 0, 0])
MAVpointLoader.add(w, comment='Joe Location')
w = self.command(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, [8, 1500, 0, 0])
MAVpointLoader.add(w, comment='Drop bottle 1')
w = self.command(mavutil.mavlink.MAV_CMD_CONDITION_DELAY, [1.5, 0, 0, 0])
MAVpointLoader.add(w, comment='Drop delay')
w = self.command(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, [12, 1050, 0, 0])
MAVpointLoader.add(w, comment='Drop bottle 2')
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, param=[0, 28, 50, 0])
MAVpointLoader.add(w, comment='Change to 28 m/s')
w = self.waypoint(*cuav_util.gps_newpos(*joe, bearing=180, distance=250), alt=dropalt)
MAVpointLoader.add(w, comment='Joe after')
w = self.jump(crosswp)
MAVpointLoader.add(w, comment='Jump to cross')
#print "Done AF home"
#WP12 - WPn - and add in the rest of the waypoints - Entry lane, search area, exit lane
entry_wpnum = MAVpointLoader.count()
for i in range(1):
point = self.entryPoints[i]
w = self.waypoint(point[0], point[1], self.airportHeight)
MAVpointLoader.add(w, comment='Entry %u' % (i+1))
endentry_wpnum = MAVpointLoader.count()
w = self.jump(0)
MAVpointLoader.add(w, comment='Jump to search mission')
# exit points
exit_wpnum = MAVpointLoader.count()
for i in range(len(self.exitPoints)):
point = self.exitPoints[i]
w = self.waypoint(point[0], point[1], self.airportHeight)
MAVpointLoader.add(w, comment='Exit point %u' % (i+1))
w = self.jump(landing_approach_wpnum)
MAVpointLoader.add(w, comment='Jump to landing approach')
# search pattern
MAVpointLoader.wp(endentry_wpnum).param1 = MAVpointLoader.count()
for i in range(len(self.SearchPattern)):
point = self.SearchPattern[i]
w = self.waypoint(point[0], point[1], point[2])
MAVpointLoader.add(w, comment='Search %u' % (i+1))
#if desired, loiter in the search area for a bit
if loiterInSearchArea == 1:
meanPoint = tuple(numpy.average(self.SearchPattern, axis=0))
w = self.waypoint(meanPoint[0], meanPoint[1], meanPoint[2],
cmd=mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
param=[600, 0, 0, 0])
MAVpointLoader.add(w, comment='Loiter in search area for 10 minutes')
exitjump.append(MAVpointLoader.count())
w = self.jump(0)
MAVpointLoader.add(w, comment='Jump to exit lane')
# fixup jump waypoint numbers
for wnum in entryjump:
MAVpointLoader.wp(wnum).param1 = entry_wpnum
for wnum in exitjump:
MAVpointLoader.wp(wnum).param1 = exit_wpnum
#export the waypoints to a MAVLink compatible format/file
waytxt = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..',
'data', opts.outname)
MAVpointLoader.save(waytxt)
print "Waypoints exported to %s" % waytxt
# create fence.txt
fenceloader = mavwp.MAVFenceLoader()
fp = mavutil.mavlink.MAVLink_fence_point_message(0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1])
fenceloader.add(fp)
for p in gen.missionBounds:
fp = mavutil.mavlink.MAVLink_fence_point_message(0, 0, 0, 0, float(p[0]), float(p[1]))
fenceloader.add(fp)
# close the polygon
p = gen.missionBounds[0]
fp = mavutil.mavlink.MAVLink_fence_point_message(0, 0, 0, 0, float(p[0]), float(p[1]))
fenceloader.add(fp)
fencetxt = os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'data', "fence.txt")
fenceloader.save(fencetxt)
print "Fence exported to %s" % fencetxt
def CreateSearchPattern(self, width=50.0, overlap=10.0, offset=10, wobble=10, alt=100):
'''Generate the waypoints for the search pattern, using alternating strips
width is the width (m) of each strip, overlap is the % overlap between strips,
alt is the altitude (relative to ground) of the points'''
self.SearchPattern = []
#find the nearest point to Airfield Home - use this as a starting point (if entry lanes are not used)
if len(self.entryPoints) == 0:
nearestdist = cuav_util.gps_distance(self.airfieldHome[0], self.airfieldHome[1], self.searchArea[0][0], self.searchArea[0][1])
nearest = self.searchArea[0]
for point in self.searchArea:
newdist = cuav_util.gps_distance(self.airfieldHome[0], self.airfieldHome[1], point[0], point[1])
if newdist < nearestdist:
nearest = point
nearestdist = newdist
else:
nearestdist = cuav_util.gps_distance(self.entryPoints[0][0], self.entryPoints[0][1], self.searchArea[0][0], self.searchArea[0][1])
nearest = self.searchArea[0]
for point in self.searchArea:
newdist = cuav_util.gps_distance(self.entryPoints[0][0], self.entryPoints[0][1], point[0], point[1])
#print "dist = " + str(newdist)
if newdist < nearestdist:
nearest = point
nearestdist = newdist
#print "Start = " + str(nearest) + ", dist = " + str(nearestdist)
#the search pattern will run between the longest side from nearest
bearing1 = cuav_util.gps_bearing(nearest[0], nearest[1], self.searchArea[self.searchArea.index(nearest)-1][0], self.searchArea[self.searchArea.index(nearest)-1][1])
bearing2 = cuav_util.gps_bearing(nearest[0], nearest[1], self.searchArea[self.searchArea.index(nearest)+1][0], self.searchArea[self.searchArea.index(nearest)+1][1])
dist1 = cuav_util.gps_distance(nearest[0], nearest[1], self.searchArea[self.searchArea.index(nearest)-1][0], self.searchArea[self.searchArea.index(nearest)-1][1])
dist2 = cuav_util.gps_distance(nearest[0], nearest[1], self.searchArea[self.searchArea.index(nearest)+1][0], self.searchArea[self.searchArea.index(nearest)+1][1])
if dist1 > dist2:
self.searchBearing = bearing1
else:
self.searchBearing = bearing2
#the search pattern will then run parallel between the two furthest points in the list
#searchLine = (0, 0)
#for point in self.searchArea:
# newdist = cuav_util.gps_distance(point[0], point[1], self.searchArea[self.searchArea.index(point)-1][0], self.searchArea[self.searchArea.index(point)-1][1])
# if newdist > searchLine[0]:
# searchLine = (newdist, cuav_util.gps_bearing(point[0], point[1], self.searchArea[self.searchArea.index(point)-1][0], self.searchArea[self.searchArea.index(point)-1][1]))
#self.searchBearing = searchLine[1]
#need to find the 90 degree bearing to searchBearing that is inside the search area. This
#will be the bearing we increment the search rows by
#need to get the right signs for the bearings, depending which quadrant the search area is in wrt nearest
if not cuav_util.polygon_outside(cuav_util.gps_newpos(nearest[0], nearest[1], (self.searchBearing + 45) % 360, 10), self.searchArea):
self.crossBearing = (self.searchBearing + 90) % 360
elif not cuav_util.polygon_outside(cuav_util.gps_newpos(nearest[0], nearest[1], (self.searchBearing + 135) % 360, 10), self.searchArea):
self.crossBearing = (self.searchBearing + 90) % 360
self.searchBearing = (self.searchBearing + 180) % 360
elif not cuav_util.polygon_outside(cuav_util.gps_newpos(nearest[0], nearest[1], (self.searchBearing - 45) % 360, 10), self.searchArea):
self.crossBearing = (self.searchBearing - 90) % 360
else:
self.crossBearing = (self.searchBearing - 90) % 360
self.searchBearing = (self.searchBearing - 180) % 360
print "Search bearing is " + str(self.searchBearing) + "/" + str((self.searchBearing + 180) % 360)
print "Cross bearing is: " + str(self.crossBearing)
#the distance between runs is this:
self.deltaRowDist = width - width*(float(overlap)/100)
if self.deltaRowDist <= 0:
print "Error, overlap % is too high"
return
print "Delta row = " + str(self.deltaRowDist)
#expand the search area to 1/2 deltaRowDist to ensure full coverage
#we are starting at the "nearest" and mowing the lawn parallel to "self.searchBearing"
#first waypoint is right near the Search Area boundary (without being on it) (10% of deltaRowDist
#on an opposite bearing (so behind the search area)
nextWaypoint = cuav_util.gps_newpos(nearest[0], nearest[1], self.crossBearing, self.deltaRowDist/10)
print "First = " + str(nextWaypoint)
#self.SearchPattern.append(firstWaypoint)
#mow the lawn, every 2nd row:
while True:
pts = self.projectBearing(self.searchBearing, nextWaypoint, self.searchArea)
#print "Projecting " + str(nextWaypoint) + " along " + str(self.searchBearing)
#check if we're outside the search area
if pts == 0:
break
(nextW, nextnextW) = (pts[0], pts[1])
if cuav_util.gps_distance(nextWaypoint[0], nextWaypoint[1], nextW[0], nextW[1]) < cuav_util.gps_distance(nextWaypoint[0], nextWaypoint[1], nextnextW[0], nextnextW[1]):
self.SearchPattern.append(cuav_util.gps_newpos(nextW[0], nextW[1], (self.searchBearing + 180) % 360, (offset+wobble)))
self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt)
self.SearchPattern.append(cuav_util.gps_newpos(nextnextW[0], nextnextW[1], self.searchBearing, (offset+wobble)))
self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt)
#now turn 90degrees from bearing and width distance
nextWaypoint = cuav_util.gps_newpos(nextnextW[0], nextnextW[1], self.crossBearing, self.deltaRowDist*2)
self.searchBearing = (self.searchBearing + 180) % 360
else:
self.SearchPattern.append(cuav_util.gps_newpos(nextnextW[0], nextnextW[1], (self.searchBearing + 180) % 360, offset+wobble))
self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt)
self.SearchPattern.append(cuav_util.gps_newpos(nextW[0], nextW[1], self.searchBearing, (offset+wobble)))
self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt)
#now turn 90degrees from bearing and width distance
nextWaypoint = cuav_util.gps_newpos(nextW[0], nextW[1], self.crossBearing, self.deltaRowDist*2)
self.searchBearing = (self.searchBearing + 180) % 360
print "Next = " + str(nextWaypoint)
#go back and do the rows we missed. There still might be one more row to do in
# the crossbearing direction, so check for that first
nextWaypoint = cuav_util.gps_newpos(nextWaypoint[0], nextWaypoint[1], self.crossBearing, -self.deltaRowDist)
pts = self.projectBearing(self.searchBearing, nextWaypoint, self.searchArea)
if pts == 0:
nextWaypoint = cuav_util.gps_newpos(nextWaypoint[0], nextWaypoint[1], self.crossBearing, -2*self.deltaRowDist)
self.crossBearing = (self.crossBearing + 180) % 360
else:
self.crossBearing = (self.crossBearing + 180) % 360
while True:
pts = self.projectBearing(self.searchBearing, nextWaypoint, self.searchArea)
#print "Projecting " + str(nextWaypoint) + " along " + str(self.searchBearing)
#check if we're outside the search area
if pts == 0:
break
(nextW, nextnextW) = (pts[0], pts[1])
if cuav_util.gps_distance(nextWaypoint[0], nextWaypoint[1], nextW[0], nextW[1]) < cuav_util.gps_distance(nextWaypoint[0], nextWaypoint[1], nextnextW[0], nextnextW[1]):
self.SearchPattern.append(cuav_util.gps_newpos(nextW[0], nextW[1], (self.searchBearing + 180) % 360, offset))
self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt)
self.SearchPattern.append(cuav_util.gps_newpos(nextnextW[0], nextnextW[1], self.searchBearing, offset))
self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt)
#now turn 90degrees from bearing and width distance
nextWaypoint = cuav_util.gps_newpos(nextnextW[0], nextnextW[1], self.crossBearing, self.deltaRowDist*2)
self.searchBearing = (self.searchBearing + 180) % 360
else:
self.SearchPattern.append(cuav_util.gps_newpos(nextnextW[0], nextnextW[1], (self.searchBearing + 180) % 360, offset))
self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt)
self.SearchPattern.append(cuav_util.gps_newpos(nextW[0], nextW[1], self.searchBearing, offset))
self.SearchPattern[-1] =(self.SearchPattern[-1][0], self.SearchPattern[-1][1], alt)
#now turn 90degrees from bearing and width distance
nextWaypoint = cuav_util.gps_newpos(nextW[0], nextW[1], self.crossBearing, self.deltaRowDist*2)
self.searchBearing = (self.searchBearing + 180) % 360
print "Next(alt) = " + str(nextWaypoint)
#add in the altitude points (relative to airfield home)
for point in self.SearchPattern:
self.SearchPattern[self.SearchPattern.index(point)] = (point[0], point[1], alt)
def altitudeCompensation(self, heightAGL, numMaxPoints=100, threshold=5):
'''Creates height points (ASL) for each point in searchArea,
entry and exit points such that the plane stays a constant altitude above the ground,
constrained by a max number of waypoints'''
maxDeltaAlt = 0
maxDeltaAltPoints = []
maxDeltapercentAlong = 0
EleModel = mp_elevation.ElevationModel()
#make sure the SRTM tiles are downloaded
EleModel.GetElevation(self.SearchPattern[0][0], self.SearchPattern[0][1])
#get the ASL height of the airfield home, entry and exit points and initial search pattern
# and add the heightAGL to them
self.airportHeight = EleModel.GetElevation(self.airfieldHome[0], self.airfieldHome[1])
if abs(opts.basealt - self.airportHeight) > 30:
print("BAD BASE ALTITUDE %u - airfieldhome %u" % (opts.basealt, self.airportHeight))
sys.exit(1)
self.airportHeight = opts.basealt
self.airfieldHome = (self.airfieldHome[0], self.airfieldHome[1], heightAGL+opts.basealt)
for point in self.entryPoints:
self.entryPoints[self.entryPoints.index(point)] = (point[0], point[1], heightAGL+10+EleModel.GetElevation(point[0], point[1]))
for point in self.exitPoints:
self.exitPoints[self.exitPoints.index(point)] = (point[0], point[1], heightAGL+10+EleModel.GetElevation(point[0], point[1]))
for point in self.SearchPattern:
self.SearchPattern[self.SearchPattern.index(point)] = (point[0], point[1], heightAGL+EleModel.GetElevation(point[0], point[1]))
#keep looping through the search area waypoints and add new waypoints where needed
#to maintain const height above terrain
print "---Starting terrain tracking optimisation---"
while True:
maxDeltaAlt = 0
maxDeltaAltPoints = []
maxDeltaPointIndex = 0
for point in self.SearchPattern:
if point != self.SearchPattern[-1]:
dist = cuav_util.gps_distance(point[0], point[1], self.SearchPattern[self.SearchPattern.index(point)+1][0], self.SearchPattern[self.SearchPattern.index(point)+1][1])
theta = cuav_util.gps_bearing(point[0], point[1], self.SearchPattern[self.SearchPattern.index(point)+1][0], self.SearchPattern[self.SearchPattern.index(point)+1][1])
AltDiff = float(self.SearchPattern[self.SearchPattern.index(point)+1][2] - point[2])
if numpy.around(theta) == numpy.around(self.searchBearing) or numpy.around(theta) == numpy.around((self.searchBearing+180) % 360):
#increment 10% along waypoint-to-waypoint and get max height difference
for i in range(1, 9):
partPoint = cuav_util.gps_newpos(point[0], point[1], theta, (i*dist/10))
partAlt = EleModel.GetElevation(partPoint[0], partPoint[1]) + heightAGL
#print "Part = " + str(partAlt) + ", Orig = " + str(point[2])
if numpy.abs(point[2] + ((AltDiff*i)/10) - partAlt) > maxDeltaAlt:
maxDeltaAlt = numpy.abs(point[2] + ((AltDiff*i)/10) - partAlt)
maxDeltaAltPoint = (partPoint[0], partPoint[1], partAlt)
maxDeltaPointIndex = self.SearchPattern.index(point)+1
#print "New max alt diff: " + str(maxDeltaAlt) + ", " + str(partAlt)
#now put a extra waypoint in between the two maxDeltaAltPoints
if maxDeltaAltPoint is not None:
self.SearchPattern.insert(maxDeltaPointIndex, maxDeltaAltPoint)
#print "There are " + str(len(self.SearchPattern)) + " points in the search pattern. Max Alt error is " + str(maxDeltaAlt)
if len(self.SearchPattern) >= numMaxPoints or threshold > maxDeltaAlt:
break
print "---Done terrain tracking optimisation---"
def update_mission(self, m):
'''update mission status'''
if not self.cuav_settings.auto_mission:
return
wpmod = self.module('wp')
wploader = wpmod.wploader
if wploader.count() < 2 and self.last_attitude_ms - self.last_wp_list_ms > 5000:
self.last_wp_list_ms = self.last_attitude_ms
wpmod.cmd_wp(["list"])
wp_start = self.find_user_wp(wploader, self.cuav_settings.wp_start)
wp_center = self.find_user_wp(wploader, self.cuav_settings.wp_center)
wp_end = self.find_user_wp(wploader, self.cuav_settings.wp_end)
if (wp_center is None or
wp_start is None or
wp_end is None):
# not configured
return
if m.seq >= wp_start and m.seq <= wp_end:
lookahead = self.cuav_settings.lookahead_search
margin_exc = self.cuav_settings.margin_exc_search
else:
lookahead = self.cuav_settings.lookahead_default
margin_exc = self.cuav_settings.margin_exc_default
v = self.mav_param.get('AVD_LOOKAHEAD', None)
if v is not None and abs(v - lookahead) > 1:
self.send_message("Set AVD_LOOKAHEAD %u" % lookahead)
self.master.param_set_send('AVD_LOOKAHEAD', lookahead)
v = self.mav_param.get('AVD_MARGIN_EXCL', None)
if v is not None and abs(v - margin_exc) > 1:
self.send_message("Set AVD_MARGIN_EXCL %u" % margin_exc)
self.master.param_set_send('AVD_MARGIN_EXCL', margin_exc)
# run every 5 seconds
if self.last_attitude_ms - self.last_mission_check_ms < 5000:
return
self.last_mission_check_ms = self.last_attitude_ms
if self.updated_waypoints:
cam = self.module('camera_air')
if wpmod.loading_waypoints:
self.send_message("listing waypoints")
wpmod.cmd_wp(["list"])
else:
self.send_message("sending waypoints")
self.updated_waypoints = False
wploader.save("newwp.txt")
cam.send_file("newwp.txt")
if self.started_landing:
# no more to do
return
if self.last_attitude_ms - self.last_wp_move_ms < 2*60*1000:
# only move waypoints every 2 minutes
return
try:
cam = self.module('camera_air')
lz = cam.lz
target_latitude = cam.camera_settings.target_latitude
target_longitude = cam.camera_settings.target_longitude
target_radius = cam.camera_settings.target_radius
except Exception:
self.send_message("target not set")
return
lzresult = lz.calclandingzone()
if lzresult is None:
return
self.send_message("lzresult nr:%u avgscore:%u" % (lzresult.numregions, lzresult.avgscore))
if lzresult.numregions < 5 and lzresult.avgscore < 20000:
# only accept short lists if they have high scores
return
(lat, lon) = lzresult.latlon
# check it is within the target radius
if target_radius > 0:
dist = cuav_util.gps_distance(lat, lon, target_latitude, target_longitude)
self.send_message("dist %u" % dist)
if dist > target_radius:
return
# don't move more than 70m from the center of the search, this keeps us
# over more of the search area, and further from the fence
max_move = target_radius
if self.wp_move_count == 0:
# don't move more than 50m from center on first move
max_move = 35
if self.wp_move_count == 1:
# don't move more than 80m from center on 2nd move
max_move = 80
if dist > max_move:
bearing = cuav_util.gps_bearing(target_latitude, target_longitude, lat, lon)
(lat, lon) = cuav_util.gps_newpos(target_latitude, target_longitude, bearing, max_move)
# we may need to fetch the wp list
if self.last_attitude_ms - self.last_wp_list_ms > 120000 or wpmod.loading_waypoints:
self.last_wp_list_ms = self.last_attitude_ms
self.send_message("fetching waypoints")
wpmod.cmd_wp(["list"])
return
self.last_wp_move_ms = self.last_attitude_ms
self.wp_move_count += 1
self.send_message("Moving search to: (%f,%f) %u" % (lat, lon, self.wp_move_count))
wpmod.cmd_wp_movemulti([wp_center, wp_start, wp_end], (lat,lon))
wp_land = self.find_user_wp(wploader, self.cuav_settings.wp_land)
if (wp_land is not None and
self.wp_move_count >= 3 and
lzresult.numregions > 10 and
self.master.flightmode == "AUTO"):
self.send_message("Starting landing")
self.master.waypoint_set_current_send(wp_land)
self.started_landing = True
self.updated_waypoints = True
def update_mission(self):
'''update mission status'''
if not self.cuav_settings.auto_mission:
return
wpmod = self.module('wp')
wploader = wpmod.wploader
if wploader.count(
) < 2 and self.last_attitude_ms - self.last_wp_list_ms > 5000:
self.last_wp_list_ms = self.last_attitude_ms
wpmod.cmd_wp(["list"])
wp_start = self.find_user_wp(wploader, self.cuav_settings.wp_start)
wp_center = self.find_user_wp(wploader, self.cuav_settings.wp_center)
wp_end = self.find_user_wp(wploader, self.cuav_settings.wp_end)
if (wp_center is None or wp_start is None or wp_end is None):
# not configured
return
# run every 5 seconds
if self.last_attitude_ms - self.last_mission_check_ms < 5000:
return
self.last_mission_check_ms = self.last_attitude_ms
if self.updated_waypoints:
cam = self.module('camera_air')
if wpmod.loading_waypoints:
self.send_message("listing waypoints")
wpmod.cmd_wp(["list"])
else:
self.send_message("sending waypoints")
self.updated_waypoints = False
wploader.save("newwp.txt")
cam.send_file("newwp.txt")
if self.started_landing:
# no more to do
return
if self.last_attitude_ms - self.last_wp_move_ms < 2 * 60 * 1000:
# only move waypoints every 2 minutes
return
try:
cam = self.module('camera_air')
lz = cam.lz
target_latitude = cam.camera_settings.target_latitude
target_longitude = cam.camera_settings.target_longitude
target_radius = cam.camera_settings.target_radius
except Exception:
self.send_message("target not set")
return
lzresult = lz.calclandingzone()
if lzresult is None:
return
self.send_message("lzresult nr:%u avgscore:%u" %
(lzresult.numregions, lzresult.avgscore))
if lzresult.numregions < 5 and lzresult.avgscore < 20000:
# only accept short lists if they have high scores
return
(lat, lon) = lzresult.latlon
# check it is within the target radius
if target_radius > 0:
dist = cuav_util.gps_distance(lat, lon, target_latitude,
target_longitude)
self.send_message("dist %u" % dist)
if dist > target_radius:
return
# don't move more than 70m from the center of the search, this keeps us
# over more of the search area, and further from the fence
max_move = target_radius
if self.wp_move_count == 0:
# don't move more than 50m from center on first move
max_move = 35
if self.wp_move_count == 1:
# don't move more than 80m from center on 2nd move
max_move = 80
if dist > max_move:
bearing = cuav_util.gps_bearing(target_latitude,
target_longitude, lat, lon)
(lat, lon) = cuav_util.gps_newpos(target_latitude,
target_longitude, bearing,
max_move)
# we may need to fetch the wp list
if self.last_attitude_ms - self.last_wp_list_ms > 120000 or wpmod.loading_waypoints:
self.last_wp_list_ms = self.last_attitude_ms
self.send_message("fetching waypoints")
wpmod.cmd_wp(["list"])
return
self.last_wp_move_ms = self.last_attitude_ms
self.wp_move_count += 1
self.send_message("Moving search to: (%f,%f) %u" %
(lat, lon, self.wp_move_count))
wpmod.cmd_wp_movemulti([wp_center, wp_start, wp_end], (lat, lon))
wp_land = self.find_user_wp(wploader, self.cuav_settings.wp_land)
if (wp_land is not None and self.wp_move_count >= 3
and lzresult.numregions > 10
and self.master.flightmode == "AUTO"):
self.send_message("Starting landing")
self.master.waypoint_set_current_send(wp_land)
self.started_landing = True
self.updated_waypoints = True
def exportToMAVProxy(self,
loiterInSearchArea=1,
basealt=100,
outname="way.txt"):
'''Exports the airfield home, entry points, search pattern and exit points to MAVProxy'''
#make a fake waypoint loader for testing purposes, if we're not
#running within MAVProxy
if self.MAVpointLoader is None:
print "No loader - creating one"
self.MAVpointLoader = mavwp.MAVWPLoader()
MAVpointLoader = self.MAVpointLoader
entryjump = []
exitjump = []
#clear out all the old waypoints
MAVpointLoader.clear()
TargetSys = MAVpointLoader.target_system
TargetComp = MAVpointLoader.target_component
#WP0 - add landingPt as waypoint 0. This gets replaced when GPS gets lock
w = self.waypoint(*self.landingPt,
alt=basealt,
frame=mavutil.mavlink.MAV_FRAME_GLOBAL)
MAVpointLoader.add(w, comment='Airfield home')
#WP2 - takeoff, then jump to entry lanes
w = self.waypoint(*cuav_util.gps_newpos(*self.landingPt,
bearing=self.landingHeading +
175,
distance=300),
alt=40,
cmd=mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
param=[12, 0, 0, 0])
MAVpointLoader.add(w, comment="Takeoff")
entryjump.append(MAVpointLoader.count())
w = self.jump(0)
MAVpointLoader.add(w, comment='Jump to entry lane')
# landing approach 1
landing_approach_wpnum = MAVpointLoader.count()
w = self.waypoint(*cuav_util.gps_newpos(*self.landingPt,
bearing=self.landingHeading +
180,
distance=1000),
alt=80)
MAVpointLoader.add(w, comment='Landing approach')
# drop our speed
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
param=[0, 23, 20, 0])
MAVpointLoader.add(w, comment='Change to 23 m/s')
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
param=[1, 23, 20, 0])
MAVpointLoader.add(w, comment='Change throttle to 20%%')
# landing approach 1
landing_approach_wpnum = MAVpointLoader.count()
w = self.waypoint(*cuav_util.gps_newpos(*self.landingPt,
bearing=self.landingHeading +
180,
distance=500),
alt=50)
MAVpointLoader.add(w, comment='Landing approach 2')
# landing
w = self.waypoint(self.landingPt[0],
self.landingPt[1],
0,
cmd=mavutil.mavlink.MAV_CMD_NAV_LAND)
MAVpointLoader.add(w, comment='Landing')
# comms Failure. Loiter at EL-4 for 2 minutes then fly to airfield home and loiter
point = self.exitPoints[1]
w = self.waypoint(point[0], point[1], self.airportHeight)
MAVpointLoader.add(w, comment='Comms Failure')
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
param=[120, 0, 0, 0])
MAVpointLoader.add(w, comment='loiter 2 minutes')
w = self.waypoint(self.airfieldHome[0], self.airfieldHome[1],
self.airportHeight)
MAVpointLoader.add(w, comment='Airfield home')
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM)
MAVpointLoader.add(w, comment='loiter')
# GPS failure. Loiter in place for 30s then direct to airfield home
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
param=[30, 0, 0, 0])
MAVpointLoader.add(w, comment='GPS fail - loiter 30 secs')
w = self.waypoint(self.airfieldHome[0], self.airfieldHome[1],
self.airportHeight)
MAVpointLoader.add(w, comment='Airfield home')
w = self.command(mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM)
MAVpointLoader.add(w, comment='loiter')
dropalt = self.joePos[2]
joe = (self.joePos[0], self.joePos[1])
crosswp = MAVpointLoader.count()
w = self.waypoint(*cuav_util.gps_newpos(*joe, bearing=60,
distance=250),
alt=dropalt)
MAVpointLoader.add(w, comment='cross 1')
w = self.waypoint(*cuav_util.gps_newpos(*joe,
bearing=240,
distance=250),
alt=dropalt)
MAVpointLoader.add(w, comment='cross 2')
w = self.waypoint(*cuav_util.gps_newpos(*joe,
bearing=300,
distance=250),
alt=dropalt)
MAVpointLoader.add(w, comment='cross 3')
w = self.waypoint(*cuav_util.gps_newpos(*joe,
bearing=120,
distance=250),
alt=dropalt)
MAVpointLoader.add(w, comment='cross 4')
w = self.jump(crosswp)
MAVpointLoader.add(w, comment='Jump to cross')
# joe approach point
w = self.waypoint(*cuav_util.gps_newpos(*joe, bearing=0, distance=350),
alt=dropalt)
MAVpointLoader.add(w, comment='Joe approach')
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
param=[0, 20, 50, 0])
MAVpointLoader.add(w, comment='Change to 20 m/s')
# joe location. We use a default acceptance radius of 35 meters. Will be adjusted with 'wp param'
# command
w = self.waypoint(*joe, alt=dropalt, param=[0, 70, 0, 0])
MAVpointLoader.add(w, comment='Joe Location')
w = self.command(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, [8, 1500, 0, 0])
MAVpointLoader.add(w, comment='Drop bottle 1')
w = self.command(mavutil.mavlink.MAV_CMD_CONDITION_DELAY,
[2.0, 0, 0, 0])
MAVpointLoader.add(w, comment='Drop delay')
w = self.command(mavutil.mavlink.MAV_CMD_DO_SET_SERVO, [6, 1050, 0, 0])
MAVpointLoader.add(w, comment='Drop bottle 2')
w = self.command(mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED,
param=[0, 28, 50, 0])
MAVpointLoader.add(w, comment='Change to 28 m/s')
w = self.waypoint(*cuav_util.gps_newpos(*joe,
bearing=180,
distance=250),
alt=dropalt)
MAVpointLoader.add(w, comment='Joe after')
w = self.jump(crosswp)
MAVpointLoader.add(w, comment='Jump to cross')
#print "Done AF home"
#WP12 - WPn - and add in the rest of the waypoints - Entry lane, search area, exit lane
entry_wpnum = MAVpointLoader.count()
for i in range(1):
point = self.entryPoints[i]
w = self.waypoint(point[0], point[1], self.airportHeight)
MAVpointLoader.add(w, comment='Entry %u' % (i + 1))
endentry_wpnum = MAVpointLoader.count()
w = self.jump(0)
MAVpointLoader.add(w, comment='Jump to search mission')
# exit points
exit_wpnum = MAVpointLoader.count()
for i in range(len(self.exitPoints)):
point = self.exitPoints[i]
w = self.waypoint(point[0], point[1], self.airportHeight)
MAVpointLoader.add(w, comment='Exit point %u' % (i + 1))
w = self.jump(landing_approach_wpnum)
MAVpointLoader.add(w, comment='Jump to landing approach')
# search pattern
MAVpointLoader.wp(endentry_wpnum).param1 = MAVpointLoader.count()
for i in range(len(self.SearchPattern)):
point = self.SearchPattern[i]
w = self.waypoint(point[0], point[1], point[2])
MAVpointLoader.add(w, comment='Search %u' % (i + 1))
#if desired, loiter in the search area for a bit
if loiterInSearchArea == 1:
meanPoint = tuple(numpy.average(self.SearchPattern, axis=0))
w = self.waypoint(meanPoint[0],
meanPoint[1],
meanPoint[2],
cmd=mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
param=[600, 0, 0, 0])
MAVpointLoader.add(w,
comment='Loiter in search area for 10 minutes')
exitjump.append(MAVpointLoader.count())
w = self.jump(0)
MAVpointLoader.add(w, comment='Jump to exit lane')
# fixup jump waypoint numbers
for wnum in entryjump:
MAVpointLoader.wp(wnum).param1 = entry_wpnum
for wnum in exitjump:
MAVpointLoader.wp(wnum).param1 = exit_wpnum
#export the waypoints to a MAVLink compatible format/file
waytxt = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'..', 'data', outname)
MAVpointLoader.save(waytxt)
print "Waypoints exported to %s" % waytxt
# create fence.txt
fenceloader = mavwp.MAVFenceLoader()
fp = mavutil.mavlink.MAVLink_fence_point_message(
0, 0, 0, 0, self.airfieldHome[0], self.airfieldHome[1])
fenceloader.add(fp)
for p in gen.missionBounds:
fp = mavutil.mavlink.MAVLink_fence_point_message(
0, 0, 0, 0, float(p[0]), float(p[1]))
fenceloader.add(fp)
# close the polygon
p = gen.missionBounds[0]
fp = mavutil.mavlink.MAVLink_fence_point_message(
0, 0, 0, 0, float(p[0]), float(p[1]))
fenceloader.add(fp)
fencetxt = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'..', 'data', "fence.txt")
fenceloader.save(fencetxt)
print "Fence exported to %s" % fencetxt
def CreateSearchPattern(self,
width=50.0,
overlap=10.0,
offset=10,
wobble=10,
alt=100):
'''Generate the waypoints for the search pattern, using alternating strips
width is the width (m) of each strip, overlap is the % overlap between strips,
alt is the altitude (relative to ground) of the points'''
self.SearchPattern = []
#find the nearest point to Airfield Home - use this as a starting point (if entry lanes are not used)
if len(self.entryPoints) == 0:
nearestdist = cuav_util.gps_distance(self.airfieldHome[0],
self.airfieldHome[1],
self.searchArea[0][0],
self.searchArea[0][1])
nearest = self.searchArea[0]
for point in self.searchArea:
newdist = cuav_util.gps_distance(self.airfieldHome[0],
self.airfieldHome[1],
point[0], point[1])
if newdist < nearestdist:
nearest = point
nearestdist = newdist
else:
nearestdist = cuav_util.gps_distance(self.entryPoints[0][0],
self.entryPoints[0][1],
self.searchArea[0][0],
self.searchArea[0][1])
nearest = self.searchArea[0]
for point in self.searchArea:
newdist = cuav_util.gps_distance(self.entryPoints[0][0],
self.entryPoints[0][1],
point[0], point[1])
#print "dist = " + str(newdist)
if newdist < nearestdist:
nearest = point
nearestdist = newdist
#print "Start = " + str(nearest) + ", dist = " + str(nearestdist)
#the search pattern will run between the longest side from nearest
bearing1 = cuav_util.gps_bearing(
nearest[0], nearest[1],
self.searchArea[self.searchArea.index(nearest) - 1][0],
self.searchArea[self.searchArea.index(nearest) - 1][1])
bearing2 = cuav_util.gps_bearing(
nearest[0], nearest[1],
self.searchArea[self.searchArea.index(nearest) + 1][0],
self.searchArea[self.searchArea.index(nearest) + 1][1])
dist1 = cuav_util.gps_distance(
nearest[0], nearest[1],
self.searchArea[self.searchArea.index(nearest) - 1][0],
self.searchArea[self.searchArea.index(nearest) - 1][1])
dist2 = cuav_util.gps_distance(
nearest[0], nearest[1],
self.searchArea[self.searchArea.index(nearest) + 1][0],
self.searchArea[self.searchArea.index(nearest) + 1][1])
if dist1 > dist2:
self.searchBearing = bearing1
else:
self.searchBearing = bearing2
#the search pattern will then run parallel between the two furthest points in the list
#searchLine = (0, 0)
#for point in self.searchArea:
# newdist = cuav_util.gps_distance(point[0], point[1], self.searchArea[self.searchArea.index(point)-1][0], self.searchArea[self.searchArea.index(point)-1][1])
# if newdist > searchLine[0]:
# searchLine = (newdist, cuav_util.gps_bearing(point[0], point[1], self.searchArea[self.searchArea.index(point)-1][0], self.searchArea[self.searchArea.index(point)-1][1]))
#self.searchBearing = searchLine[1]
#need to find the 90 degree bearing to searchBearing that is inside the search area. This
#will be the bearing we increment the search rows by
#need to get the right signs for the bearings, depending which quadrant the search area is in wrt nearest
if not cuav_util.polygon_outside(
cuav_util.gps_newpos(nearest[0], nearest[1],
(self.searchBearing + 45) % 360, 10),
self.searchArea):
self.crossBearing = (self.searchBearing + 90) % 360
elif not cuav_util.polygon_outside(
cuav_util.gps_newpos(nearest[0], nearest[1],
(self.searchBearing + 135) % 360, 10),
self.searchArea):
self.crossBearing = (self.searchBearing + 90) % 360
self.searchBearing = (self.searchBearing + 180) % 360
elif not cuav_util.polygon_outside(
cuav_util.gps_newpos(nearest[0], nearest[1],
(self.searchBearing - 45) % 360, 10),
self.searchArea):
self.crossBearing = (self.searchBearing - 90) % 360
else:
self.crossBearing = (self.searchBearing - 90) % 360
self.searchBearing = (self.searchBearing - 180) % 360
print "Search bearing is " + str(self.searchBearing) + "/" + str(
(self.searchBearing + 180) % 360)
print "Cross bearing is: " + str(self.crossBearing)
#the distance between runs is this:
self.deltaRowDist = width - width * (float(overlap) / 100)
if self.deltaRowDist <= 0:
print "Error, overlap % is too high"
return
print "Delta row = " + str(self.deltaRowDist)
#expand the search area to 1/2 deltaRowDist to ensure full coverage
#we are starting at the "nearest" and mowing the lawn parallel to "self.searchBearing"
#first waypoint is right near the Search Area boundary (without being on it) (10% of deltaRowDist
#on an opposite bearing (so behind the search area)
nextWaypoint = cuav_util.gps_newpos(nearest[0], nearest[1],
self.crossBearing,
self.deltaRowDist / 10)
print "First = " + str(nextWaypoint)
#self.SearchPattern.append(firstWaypoint)
#mow the lawn, every 2nd row:
while True:
pts = self.projectBearing(self.searchBearing, nextWaypoint,
self.searchArea)
#print "Projecting " + str(nextWaypoint) + " along " + str(self.searchBearing)
#check if we're outside the search area
if pts == 0:
break
(nextW, nextnextW) = (pts[0], pts[1])
if cuav_util.gps_distance(
nextWaypoint[0], nextWaypoint[1],
nextW[0], nextW[1]) < cuav_util.gps_distance(
nextWaypoint[0], nextWaypoint[1], nextnextW[0],
nextnextW[1]):
self.SearchPattern.append(
cuav_util.gps_newpos(nextW[0], nextW[1],
(self.searchBearing + 180) % 360,
(offset + wobble)))
self.SearchPattern[-1] = (self.SearchPattern[-1][0],
self.SearchPattern[-1][1], alt)
self.SearchPattern.append(
cuav_util.gps_newpos(nextnextW[0], nextnextW[1],
self.searchBearing,
(offset + wobble)))
self.SearchPattern[-1] = (self.SearchPattern[-1][0],
self.SearchPattern[-1][1], alt)
#now turn 90degrees from bearing and width distance
nextWaypoint = cuav_util.gps_newpos(nextnextW[0], nextnextW[1],
self.crossBearing,
self.deltaRowDist * 2)
self.searchBearing = (self.searchBearing + 180) % 360
else:
self.SearchPattern.append(
cuav_util.gps_newpos(nextnextW[0], nextnextW[1],
(self.searchBearing + 180) % 360,
offset + wobble))
self.SearchPattern[-1] = (self.SearchPattern[-1][0],
self.SearchPattern[-1][1], alt)
self.SearchPattern.append(
cuav_util.gps_newpos(nextW[0], nextW[1],
self.searchBearing,
(offset + wobble)))
self.SearchPattern[-1] = (self.SearchPattern[-1][0],
self.SearchPattern[-1][1], alt)
#now turn 90degrees from bearing and width distance
nextWaypoint = cuav_util.gps_newpos(nextW[0], nextW[1],
self.crossBearing,
self.deltaRowDist * 2)
self.searchBearing = (self.searchBearing + 180) % 360
print "Next = " + str(nextWaypoint)
#go back and do the rows we missed. There still might be one more row to do in
# the crossbearing direction, so check for that first
nextWaypoint = cuav_util.gps_newpos(nextWaypoint[0], nextWaypoint[1],
self.crossBearing,
-self.deltaRowDist)
pts = self.projectBearing(self.searchBearing, nextWaypoint,
self.searchArea)
if pts == 0:
nextWaypoint = cuav_util.gps_newpos(nextWaypoint[0],
nextWaypoint[1],
self.crossBearing,
-2 * self.deltaRowDist)
self.crossBearing = (self.crossBearing + 180) % 360
else:
self.crossBearing = (self.crossBearing + 180) % 360
while True:
pts = self.projectBearing(self.searchBearing, nextWaypoint,
self.searchArea)
#print "Projecting " + str(nextWaypoint) + " along " + str(self.searchBearing)
#check if we're outside the search area
if pts == 0:
break
(nextW, nextnextW) = (pts[0], pts[1])
if cuav_util.gps_distance(
nextWaypoint[0], nextWaypoint[1],
nextW[0], nextW[1]) < cuav_util.gps_distance(
nextWaypoint[0], nextWaypoint[1], nextnextW[0],
nextnextW[1]):
self.SearchPattern.append(
cuav_util.gps_newpos(nextW[0], nextW[1],
(self.searchBearing + 180) % 360,
offset))
self.SearchPattern[-1] = (self.SearchPattern[-1][0],
self.SearchPattern[-1][1], alt)
self.SearchPattern.append(
cuav_util.gps_newpos(nextnextW[0], nextnextW[1],
self.searchBearing, offset))
self.SearchPattern[-1] = (self.SearchPattern[-1][0],
self.SearchPattern[-1][1], alt)
#now turn 90degrees from bearing and width distance
nextWaypoint = cuav_util.gps_newpos(nextnextW[0], nextnextW[1],
self.crossBearing,
self.deltaRowDist * 2)
self.searchBearing = (self.searchBearing + 180) % 360
else:
self.SearchPattern.append(
cuav_util.gps_newpos(nextnextW[0], nextnextW[1],
(self.searchBearing + 180) % 360,
offset))
self.SearchPattern[-1] = (self.SearchPattern[-1][0],
self.SearchPattern[-1][1], alt)
self.SearchPattern.append(
cuav_util.gps_newpos(nextW[0], nextW[1],
self.searchBearing, offset))
self.SearchPattern[-1] = (self.SearchPattern[-1][0],
self.SearchPattern[-1][1], alt)
#now turn 90degrees from bearing and width distance
nextWaypoint = cuav_util.gps_newpos(nextW[0], nextW[1],
self.crossBearing,
self.deltaRowDist * 2)
self.searchBearing = (self.searchBearing + 180) % 360
print "Next(alt) = " + str(nextWaypoint)
#add in the altitude points (relative to airfield home)
for point in self.SearchPattern:
self.SearchPattern[self.SearchPattern.index(point)] = (point[0],
point[1],
alt)