コード例 #1
0
ファイル: playimages.py プロジェクト: weihli/cuav
 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)
コード例 #2
0
ファイル: playimages.py プロジェクト: 1ee7/cuav
 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)
コード例 #3
0
ファイル: mav_position.py プロジェクト: stephendade/cuav
    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)
コード例 #4
0
    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)
コード例 #5
0
ファイル: mav_position.py プロジェクト: kubark42/cuav
    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)
コード例 #6
0
    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)
コード例 #7
0
ファイル: agl_mission.py プロジェクト: ranqingfa/cuav
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
コード例 #8
0
ファイル: agl_mission.py プロジェクト: weihli/cuav
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
コード例 #9
0
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
コード例 #10
0
ファイル: mav_position.py プロジェクト: jaxiano/cuav
	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)
コード例 #11
0
ファイル: agl_mission.py プロジェクト: weihli/cuav
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
コード例 #12
0
ファイル: agl_mission.py プロジェクト: ranqingfa/cuav
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
コード例 #13
0
ファイル: cuav_missiongenerator.py プロジェクト: laetic/cuav
    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---"
コード例 #14
0
ファイル: cuav_missiongenerator.py プロジェクト: mridley/cuav
    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
コード例 #15
0
ファイル: cuav_missiongenerator.py プロジェクト: mridley/cuav
    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)
コード例 #16
0
    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---"
コード例 #17
0
ファイル: cuav_companion.py プロジェクト: CanberraUAV/cuav
    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
コード例 #18
0
    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
コード例 #19
0
    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
コード例 #20
0
    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)