Exemple #1
0
    def area_to_tile_list(self, lat, lon, width, height, ground_width, zoom=None):
        '''return a list of TileInfoScaled objects needed for
        an area of land, with ground_width in meters, and
        width/height in pixels.

        lat/lon is the top left corner. If unspecified, the
        zoom is automatically chosen to avoid having to grow
        the tiles
        '''

        pixel_width = ground_width / float(width)
        ground_height = ground_width * (height/(float(width)))
        top_right = mp_util.gps_newpos(lat, lon, 90, ground_width)
        bottom_left = mp_util.gps_newpos(lat, lon, 180, ground_height)
        ground_width_bottom = ground_width * cos(radians(lat)) / max(1.0e-15,cos(radians(bottom_left[0])))
        bottom_right = mp_util.gps_newpos(bottom_left[0], bottom_left[1], 90, ground_width_bottom)

        # choose a zoom level if not provided
        if zoom is None:
            zooms = range(self.min_zoom, self.max_zoom+1)
        else:
            zooms = [zoom]
        for zoom in zooms:
            tile_min = self.coord_to_tile(lat, lon, zoom)
            (twidth,theight) = tile_min.size()
            tile_pixel_width = twidth / float(TILES_WIDTH)
            scale = pixel_width / tile_pixel_width
            if scale >= 1.0:
                break

        scaled_tile_width = int(TILES_WIDTH / scale)
        scaled_tile_height = int(TILES_HEIGHT / scale)

        # work out the bottom right tile
        tile_max = self.coord_to_tile(bottom_right[0], bottom_right[1], zoom)

        ofsx = int(tile_min.offsetx / scale)
        ofsy = int(tile_min.offsety / scale)
        srcy = ofsy
        dsty = 0

        ret = []

        # place the tiles
        for y in range(tile_min.y, tile_max.y+1):
            srcx = ofsx
            dstx = 0
            world_tiles = 1<<zoom
            lim_x = (tile_max.x+1) % world_tiles
            x = tile_min.x
            while x != lim_x:
                if dstx < width and dsty < height:
                    ret.append(TileInfoScaled((x,y), zoom, scale,
                                                                  (srcx,srcy), (dstx,dsty), self.service))
                dstx += scaled_tile_width-srcx
                srcx = 0
                x = (x+1) % world_tiles
            dsty += scaled_tile_height-srcy
            srcy = 0
        return ret
Exemple #2
0
    def draw(self, img, pixmapper, bounds):
        '''draw a polygon on the image'''
        if self.hidden:
            return
        (lat,lon,w,h) = bounds
        # note that w and h are in degrees
        spacing = 1000
        while True:
            start = mp_util.latlon_round((lat,lon), spacing)
            lat2 = mp_util.constrain(lat+h*0.5,-85,85)
            lon2 = mp_util.wrap_180(lon+w)
            dist = mp_util.gps_distance(lat2,lon,lat2,lon2)
            count = int(dist / spacing)
            if count < 2:
                spacing /= 10
            elif count > 50:
                spacing *= 10
            else:
                break

        count += 10

        for i in range(count):
            # draw vertical lines of constant longitude
            pos1 = mp_util.gps_newpos(start[0], start[1], 90, i*spacing)
            pos3 = (pos1[0]+h*2, pos1[1])
            self.draw_line(img, pixmapper, pos1, pos3, self.colour, self.linewidth)

            # draw horizontal lines of constant latitude
            pos1 = mp_util.gps_newpos(start[0], start[1], 0, i*spacing)
            pos3 = (pos1[0], pos1[1]+w*2)
            self.draw_line(img, pixmapper, pos1, pos3, self.colour, self.linewidth)
    def draw(self, img, pixmapper, bounds):
        '''draw a polygon on the image'''
        if self.hidden:
            return
        (x, y, w, h) = bounds
        spacing = 1000
        while True:
            start = mp_util.latlon_round((x, y), spacing)
            dist = mp_util.gps_distance(x, y, x + w, y + h)
            count = int(dist / spacing)
            if count < 2:
                spacing /= 10
            elif count > 50:
                spacing *= 10
            else:
                break

        for i in range(count * 2 + 2):
            pos1 = mp_util.gps_newpos(start[0], start[1], 90, i * spacing)
            pos3 = mp_util.gps_newpos(pos1[0], pos1[1], 0, 3 * count * spacing)
            self.draw_line(img, pixmapper, pos1, pos3, self.colour,
                           self.linewidth)

            pos1 = mp_util.gps_newpos(start[0], start[1], 0, i * spacing)
            pos3 = mp_util.gps_newpos(pos1[0], pos1[1], 90,
                                      3 * count * spacing)
            self.draw_line(img, pixmapper, pos1, pos3, self.colour,
                           self.linewidth)
    def draw(self, img, pixmapper, bounds):
        '''draw a polygon on the image'''
        if self.hidden:
            return
	(x,y,w,h) = bounds
        spacing = 1000
        while True:
            start = mp_util.latlon_round((x,y), spacing)
            dist = mp_util.gps_distance(x,y,x+w,y+h)
            count = int(dist / spacing)
            if count < 2:
                spacing /= 10
            elif count > 50:
                spacing *= 10
            else:
                break

        for i in range(count*2+2):
            pos1 = mp_util.gps_newpos(start[0], start[1], 90, i*spacing)
            pos3 = mp_util.gps_newpos(pos1[0], pos1[1], 0, 3*count*spacing)
            self.draw_line(img, pixmapper, pos1, pos3, self.colour, self.linewidth)

            pos1 = mp_util.gps_newpos(start[0], start[1], 0, i*spacing)
            pos3 = mp_util.gps_newpos(pos1[0], pos1[1], 90, 3*count*spacing)
            self.draw_line(img, pixmapper, pos1, pos3, self.colour, self.linewidth)
Exemple #5
0
	def area_to_tile_list(self, lat, lon, width, height, ground_width, zoom=None):
		'''return a list of TileInfoScaled objects needed for
		an area of land, with ground_width in meters, and
		width/height in pixels.

		lat/lon is the top left corner. If unspecified, the
		zoom is automatically chosen to avoid having to grow
		the tiles
		'''

		pixel_width = ground_width / float(width)
		ground_height = ground_width * (height/(float(width)))
		top_right = mp_util.gps_newpos(lat, lon, 90, ground_width)
		bottom_left = mp_util.gps_newpos(lat, lon, 180, ground_height)
		bottom_right = mp_util.gps_newpos(bottom_left[0], bottom_left[1], 90, ground_width)

		# choose a zoom level if not provided
		if zoom is None:
			zooms = range(self.min_zoom, self.max_zoom+1)
		else:
			zooms = [zoom]
		for zoom in zooms:
			tile_min = self.coord_to_tile(lat, lon, zoom)
			(twidth,theight) = tile_min.size()
			tile_pixel_width = twidth / float(TILES_WIDTH)
			scale = pixel_width / tile_pixel_width
			if scale >= 1.0:
				break

		scaled_tile_width = int(TILES_WIDTH / scale)
		scaled_tile_height = int(TILES_HEIGHT / scale)

		# work out the bottom right tile
		tile_max = self.coord_to_tile(bottom_right[0], bottom_right[1], zoom)

		ofsx = int(tile_min.offsetx / scale)
		ofsy = int(tile_min.offsety / scale)
		srcy = ofsy
		dsty = 0

		ret = []

		# place the tiles
		for y in range(tile_min.y, tile_max.y+1):
			srcx = ofsx
			dstx = 0
			for x in range(tile_min.x, tile_max.x+1):
				if dstx < width and dsty < height:
					ret.append(TileInfoScaled((x,y), zoom, scale,
                                                                  (srcx,srcy), (dstx,dsty), self.service))
				dstx += scaled_tile_width-srcx
				srcx = 0
			dsty += scaled_tile_height-srcy
			srcy = 0
		return ret
    def update_target(self, time_boot_ms):
        '''update target on map'''
        if not self.mpstate.map:
            # don't draw if no map
            return
        if not 'HOME_POSITION' in self.master.messages:
            return

        home_position = self.master.messages['HOME_POSITION']

        now = time_boot_ms * 1.0e-3
        dt = now - self.last_update
        if dt < 0:
            dt = 0
        self.last_update = now

        self.circle_dist += dt * self.follow_settings.speed

        # assume a circle for now
        circumference = math.pi * self.follow_settings.radius * 2
        rotations = math.fmod(self.circle_dist, circumference) / circumference
        angle = math.pi * 2 * rotations
        self.target_pos = mp_util.gps_newpos(home_position.latitude*1.0e-7,
                                             home_position.longitude*1.0e-7,
                                             math.degrees(angle),
                                             self.follow_settings.radius)

        icon = self.mpstate.map.icon('camera-small-red.png')
        (lat, lon) = (self.target_pos[0], self.target_pos[1])
        self.mpstate.map.add_object(mp_slipmap.SlipIcon('followtest',
                                                        (lat, lon),
                                                        icon, layer='FollowTest', rotation=0, follow=False))
Exemple #7
0
    def cmd_wp_move_rel_home(self, args, latlon=None):
        '''handle wp move to a point relative to home by dist/bearing'''
        if len(args) < 3:
            print("usage: wp moverelhome WPNUM dist bearing")
            return
        idx = int(args[0])
        if idx < 1 or idx > self.wploader.count():
            print("Invalid wp number %u" % idx)
            return
        dist = float(args[1])
        bearing = float(args[2])

        home = self.get_home()
        if home is None:
            print("Need home")
            return

        wp = self.wploader.wp(idx)
        if not self.wploader.is_location_command(wp.command):
            print("Not a nav command")
            return
        (newlat, newlon) = mp_util.gps_newpos(home.x, home.y, bearing, dist)
        wp.x = newlat
        wp.y = newlon
        wp.target_system    = self.target_system
        wp.target_component = self.target_component
        self.wploader.set(wp, idx)

        self.loading_waypoints = True
        self.loading_waypoint_lasttime = time.time()
        self.master.mav.mission_write_partial_list_send(self.target_system,
                                                        self.target_component,
                                                        idx, idx+1)
        print("Moved WP %u %.1fm bearing %.1f from home" % (idx, dist, bearing))
Exemple #8
0
    def on_mouse(self, event):
        '''handle mouse events'''
        state = self.state
        pos = event.GetPosition()
        if event.Leaving():
            self.mouse_pos = None
        else:
            self.mouse_pos = pos
        self.update_position()

        if event.ButtonIsDown(wx.MOUSE_BTN_ANY) or event.ButtonUp():
            # send any event with a mouse button to the parent
            latlon = self.coordinates(pos.x, pos.y)
            selected = self.selected_objects(pos)
            state.event_queue.put(SlipMouseEvent(latlon, event, selected))

        if event.LeftDown():
            self.mouse_down = pos
            self.last_click_pos = self.click_pos
            self.click_pos = self.coordinates(pos.x, pos.y)

        if event.Dragging() and event.ButtonIsDown(wx.MOUSE_BTN_LEFT):
            # drag map to new position
            newpos = pos
            dx = (self.mouse_down.x - newpos.x)
            dy = -(self.mouse_down.y - newpos.y)
            pdist = math.sqrt(dx**2 + dy**2)
            if pdist > state.drag_step:
                bearing = math.degrees(math.atan2(dx, dy))
                distance = (state.ground_width/float(state.width)) * pdist
                newlatlon = mp_util.gps_newpos(state.lat, state.lon, bearing, distance)
                (state.lat, state.lon) = newlatlon
                self.mouse_down = newpos
                self.redraw_map()
Exemple #9
0
    def on_mouse(self, event):
        '''handle mouse events'''
        state = self.state
        pos = event.GetPosition()
        if event.Leaving():
            self.mouse_pos = None
        else:
            self.mouse_pos = pos
        self.update_position()

        if event.ButtonIsDown(wx.MOUSE_BTN_ANY) or event.ButtonUp():
            # send any event with a mouse button to the parent
            latlon = self.coordinates(pos.x, pos.y)
            selected = self.selected_objects(pos)
            state.event_queue.put(SlipMouseEvent(latlon, event, selected))

        if event.LeftDown():
            self.mouse_down = pos
            self.last_click_pos = self.click_pos
            self.click_pos = self.coordinates(pos.x, pos.y)

        if event.Dragging() and event.ButtonIsDown(wx.MOUSE_BTN_LEFT):
            # drag map to new position
            newpos = pos
            dx = (self.mouse_down.x - newpos.x)
            dy = -(self.mouse_down.y - newpos.y)
            pdist = math.sqrt(dx**2 + dy**2)
            if pdist > state.drag_step:
                bearing = math.degrees(math.atan2(dx, dy))
                distance = (state.ground_width / float(state.width)) * pdist
                newlatlon = mp_util.gps_newpos(state.lat, state.lon, bearing,
                                               distance)
                (state.lat, state.lon) = newlatlon
                self.mouse_down = newpos
                self.redraw_map()
    def update_target(self, time_boot_ms):
        '''update target on map'''
        if not self.mpstate.map:
            # don't draw if no map
            return
        if not 'HOME_POSITION' in self.master.messages:
            return

        home_position = self.master.messages['HOME_POSITION']

        now = time_boot_ms * 1.0e-3
        dt = now - self.last_update
        if dt < 0:
            dt = 0
        self.last_update = now

        self.circle_dist += dt * self.follow_settings.speed

        # assume a circle for now
        circumference = math.pi * self.follow_settings.radius * 2
        rotations = math.fmod(self.circle_dist, circumference) / circumference
        angle = math.pi * 2 * rotations
        self.target_pos = mp_util.gps_newpos(home_position.latitude * 1.0e-7,
                                             home_position.longitude * 1.0e-7,
                                             math.degrees(angle),
                                             self.follow_settings.radius)

        icon = self.mpstate.map.icon('camera-small-red.png')
        (lat, lon) = (self.target_pos[0], self.target_pos[1])
        self.mpstate.map.add_object(
            mp_slipmap.SlipIcon('followtest', (lat, lon),
                                icon,
                                layer='FollowTest',
                                rotation=0,
                                follow=False))
Exemple #11
0
 def re_center(self, x, y, lat, lon):
     '''re-center view for pixel x,y'''
     state = self.state
     if lat is None or lon is None:
         return
     (lat2,lon2) = self.coordinates(x, y)
     distance = mp_util.gps_distance(lat2, lon2, lat, lon)
     bearing  = mp_util.gps_bearing(lat2, lon2, lat, lon)
     (state.lat, state.lon) = mp_util.gps_newpos(state.lat, state.lon, bearing, distance)
Exemple #12
0
 def re_center(self, x, y, lat, lon):
     '''re-center view for pixel x,y'''
     state = self.state
     if lat is None or lon is None:
         return
     (lat2,lon2) = self.coordinates(x, y)
     distance = mp_util.gps_distance(lat2, lon2, lat, lon)
     bearing  = mp_util.gps_bearing(lat2, lon2, lat, lon)
     (state.lat, state.lon) = mp_util.gps_newpos(state.lat, state.lon, bearing, distance)
def mavlink_packet(m):
    '''handle an incoming mavlink packet'''
    state = mpstate.cameraview_state
    if m.get_type() == 'GLOBAL_POSITION_INT':
        state.lat, state.lon = m.lat*scale_latlon, m.lon*scale_latlon
        state.hdg = m.hdg*scale_hdg
        state.height = m.relative_alt*scale_relative_alt + state.home_height - state.elevation_model.GetElevation(state.lat, state.lon)
    elif m.get_type() == 'ATTITUDE':
        state.roll, state.pitch, state.yaw = math.degrees(m.roll), math.degrees(m.pitch), math.degrees(m.yaw)
    elif m.get_type() in ['GPS_RAW', 'GPS_RAW_INT']:
        if mpstate.wp_state.wploader.count() > 0:
            home = mpstate.wp_state.wploader.wp(0).x, mpstate.wp_state.wploader.wp(0).y
        else:
            home = [mpstate.master().field('HOME', c)*scale_latlon for c in ['lat', 'lon']]
        old = state.home_height # TODO TMP
        state.home_height = state.elevation_model.GetElevation(*home)

        # TODO TMP
        if state.home_height != old:
            # tridge said to get home pos from wploader,
            # but this is not the same as from master() below...!!
            # using master() gives the right coordinates
            # (i.e. matches GLOBAL_POSITION_INT coords, and $IMHOME in sim_arduplane.sh)
            # and wploader is a bit off
            print 'home height changed from',old,'to',state.home_height
    elif m.get_type() == 'SERVO_OUTPUT_RAW':
        for (axis, attr) in [('ROLL', 'mount_roll'), ('TILT', 'mount_pitch'), ('PAN', 'mount_yaw')]:
            channel = int(mpstate.mav_param.get('MNT_RC_IN_{0}'.format(axis), 0))
            if mpstate.mav_param.get('MNT_STAB_{0}'.format(axis), 0) and channel:
                # enabled stabilisation on this axis
                # TODO just guessing that RC_IN_ROLL gives the servo number, but no idea if this is really the case
                servo = 'servo{0}_raw'.format(channel)
                centidegrees = scale_rc(getattr(m, servo),
                                        mpstate.mav_param.get('MNT_ANGMIN_{0}'.format(axis[:3])),
                                        mpstate.mav_param.get('MNT_ANGMAX_{0}'.format(axis[:3])),
                                        param='RC{0}'.format(channel))
                setattr(state, attr, centidegrees*0.01)
        #state.mount_roll = min(max(-state.roll,-45),45)#TODO TMP
        #state.mount_yaw = min(max(-state.yaw,-45),45)#TODO TMP
        #state.mount_pitch = min(max(-state.pitch,-45),45)#TODO TMP
    else:
        return
    if mpstate.map: # if the map module is loaded, redraw polygon
        # get rid of the old polygon
        mpstate.map.add_object(mp_slipmap.SlipClearLayer('CameraView'))

        # camera view polygon determined by projecting corner pixels of the image onto the ground
        pixel_positions = [cuav_util.pixel_position(px[0],px[1], state.height, state.pitch+state.mount_pitch, state.roll+state.mount_roll, state.yaw+state.mount_yaw, state.camera_params) for px in [(0,0), (state.camera_params.xresolution,0), (state.camera_params.xresolution,state.camera_params.yresolution), (0,state.camera_params.yresolution)]]
        if any(pixel_position is None for pixel_position in pixel_positions):
            # at least one of the pixels is not on the ground
            # so it doesn't make sense to try to draw the polygon
            return
        gps_positions = [mp_util.gps_newpos(state.lat, state.lon, math.degrees(math.atan2(*pixel_position)), math.hypot(*pixel_position)) for pixel_position in pixel_positions]

        # draw new polygon
        mpstate.map.add_object(mp_slipmap.SlipPolygon('cameraview', gps_positions+[gps_positions[0]], # append first element to close polygon
                                                      layer='CameraView', linewidth=2, colour=state.col))
 def mavlink_packet(self, m):
     '''handle an incoming mavlink packet'''
     state = self
     if m.get_type() == 'GLOBAL_POSITION_INT':
         state.lat, state.lon = m.lat*scale_latlon, m.lon*scale_latlon
         state.hdg = m.hdg*scale_hdg
         state.height = m.relative_alt*scale_relative_alt + state.home_height - state.elevation_model.GetElevation(state.lat, state.lon)
     elif m.get_type() == 'ATTITUDE':
         state.roll, state.pitch, state.yaw = math.degrees(m.roll), math.degrees(m.pitch), math.degrees(m.yaw)
     elif m.get_type() in ['GPS_RAW', 'GPS_RAW_INT']:
         if self.module('wp').wploader.count() > 0:
             home = self.module('wp').wploader.wp(0).x, self.module('wp').wploader.wp(0).y
         else:
             home = [self.master.field('HOME', c)*scale_latlon for c in ['lat', 'lon']]
         old = state.home_height # TODO TMP
         state.home_height = state.elevation_model.GetElevation(*home)
 
         # TODO TMP
         if state.home_height != old:
             # tridge said to get home pos from wploader,
             # but this is not the same as from master() below...!!
             # using master() gives the right coordinates
             # (i.e. matches GLOBAL_POSITION_INT coords, and $IMHOME in sim_arduplane.sh)
             # and wploader is a bit off
             print 'home height changed from',old,'to',state.home_height
     elif m.get_type() == 'SERVO_OUTPUT_RAW':
         for (axis, attr) in [('ROLL', 'mount_roll'), ('TILT', 'mount_pitch'), ('PAN', 'mount_yaw')]:
             channel = int(self.get_mav_param('MNT_RC_IN_{0}'.format(axis), 0))
             if self.get_mav_param('MNT_STAB_{0}'.format(axis), 0) and channel:
                 # enabled stabilisation on this axis
                 # TODO just guessing that RC_IN_ROLL gives the servo number, but no idea if this is really the case
                 servo = 'servo{0}_raw'.format(channel)
                 centidegrees = self.scale_rc(getattr(m, servo),
                                         self.get_mav_param('MNT_ANGMIN_{0}'.format(axis[:3])),
                                         self.get_mav_param('MNT_ANGMAX_{0}'.format(axis[:3])),
                                         param='RC{0}'.format(channel))
                 setattr(state, attr, centidegrees*0.01)
         #state.mount_roll = min(max(-state.roll,-45),45)#TODO TMP
         #state.mount_yaw = min(max(-state.yaw,-45),45)#TODO TMP
         #state.mount_pitch = min(max(-state.pitch,-45),45)#TODO TMP
     else:
         return
     if self.mpstate.map: # if the map module is loaded, redraw polygon
         # get rid of the old polygon
         self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('CameraView'))
 
         # camera view polygon determined by projecting corner pixels of the image onto the ground
         pixel_positions = [cuav_util.pixel_position(px[0],px[1], state.height, state.pitch+state.mount_pitch, state.roll+state.mount_roll, state.yaw+state.mount_yaw, state.camera_params) for px in [(0,0), (state.camera_params.xresolution,0), (state.camera_params.xresolution,state.camera_params.yresolution), (0,state.camera_params.yresolution)]]
         if any(pixel_position is None for pixel_position in pixel_positions):
             # at least one of the pixels is not on the ground
             # so it doesn't make sense to try to draw the polygon
             return
         gps_positions = [mp_util.gps_newpos(state.lat, state.lon, math.degrees(math.atan2(*pixel_position)), math.hypot(*pixel_position)) for pixel_position in pixel_positions]
 
         # draw new polygon
         self.mpstate.map.add_object(mp_slipmap.SlipPolygon('cameraview', gps_positions+[gps_positions[0]], # append first element to close polygon
                                                       layer='CameraView', linewidth=2, colour=state.col))
Exemple #15
0
    def on_mouse(self, event):
        '''handle mouse events'''
        state = self.state
        pos = event.GetPosition()
        if event.Leaving():
            self.mouse_pos = None
        else:
            self.mouse_pos = pos
        self.update_position()

        if event.ButtonIsDown(wx.MOUSE_BTN_ANY) or event.ButtonUp():
            # send any event with a mouse button to the parent
            latlon = self.coordinates(pos.x, pos.y)
            selected = self.selected_objects(pos)
            state.event_queue.put(SlipMouseEvent(latlon, event, selected))
            if event.RightDown():
                state.popup_object = None
                state.popup_latlon = None
                if len(selected) > 0:
                    obj = state.layers[selected[0].layer][selected[0].objkey]
                    if obj.popup_menu is not None:
                        state.popup_object = obj
                        state.popup_latlon = latlon
                        self.show_popup(obj, pos)
                        state.popup_started = True
                if not state.popup_started and state.default_popup is not None:
                    state.popup_latlon = latlon
                    self.show_default_popup(pos)
                    state.popup_started = True

        if not event.ButtonIsDown(wx.MOUSE_BTN_RIGHT):
            state.popup_started = False

        if event.LeftDown() or event.RightDown():
            self.mouse_down = pos
            self.last_click_pos = self.click_pos
            self.click_pos = self.coordinates(pos.x, pos.y)

        if event.Dragging() and event.ButtonIsDown(wx.MOUSE_BTN_LEFT):
            # drag map to new position
            newpos = pos
            if self.mouse_down and newpos:
                dx = (self.mouse_down.x - newpos.x)
                dy = -(self.mouse_down.y - newpos.y)
                pdist = math.sqrt(dx**2 + dy**2)
                if pdist > state.drag_step:
                    bearing = math.degrees(math.atan2(dx, dy))
                    distance = (state.ground_width /
                                float(state.width)) * pdist
                    newlatlon = mp_util.gps_newpos(state.lat, state.lon,
                                                   bearing, distance)
                    (state.lat, state.lon) = newlatlon
                    self.mouse_down = newpos
                    self.redraw_map()
Exemple #16
0
    def on_mouse(self, event):
        '''handle mouse events'''
        state = self.state
        pos = event.GetPosition()
        if event.Leaving():
            self.mouse_pos = None
        else:
            self.mouse_pos = pos
        self.update_position()

        if event.ButtonIsDown(wx.MOUSE_BTN_ANY) or event.ButtonUp():
            # send any event with a mouse button to the parent
            latlon = self.coordinates(pos.x, pos.y)
            selected = self.selected_objects(pos)
            state.event_queue.put(SlipMouseEvent(latlon, event, selected))
            if event.RightDown():
                state.popup_object = None
                state.popup_latlon = None
                if len(selected) > 0:
                    obj = state.layers[selected[0].layer][selected[0].objkey]
                    if obj.popup_menu is not None:
                        state.popup_object = obj
                        state.popup_latlon = latlon
                        self.show_popup(obj, pos)
                        state.popup_started = True
                if not state.popup_started and state.default_popup is not None:
                    state.popup_latlon = latlon
                    self.show_default_popup(pos)
                    state.popup_started = True

        if not event.ButtonIsDown(wx.MOUSE_BTN_RIGHT):
            state.popup_started = False

        if event.LeftDown() or event.RightDown():
            self.mouse_down = pos
            self.last_click_pos = self.click_pos
            self.click_pos = self.coordinates(pos.x, pos.y)

        if event.Dragging() and event.ButtonIsDown(wx.MOUSE_BTN_LEFT):
            # drag map to new position
            newpos = pos
            if self.mouse_down and newpos:
                dx = (self.mouse_down.x - newpos.x)
                dy = -(self.mouse_down.y - newpos.y)
                pdist = math.sqrt(dx**2 + dy**2)
                if pdist > state.drag_step:
                    bearing = math.degrees(math.atan2(dx, dy))
                    distance = (state.ground_width/float(state.width)) * pdist
                    newlatlon = mp_util.gps_newpos(state.lat, state.lon, bearing, distance)
                    (state.lat, state.lon) = newlatlon
                    self.mouse_down = newpos
                    self.redraw_map()
Exemple #17
0
def find_location_by_name(autotest, locname):
    """Search locations.txt for locname, return GPS coords"""
    locations_userpath = os.environ.get('ARDUPILOT_LOCATIONS',
                                        get_user_locations_path())
    locations_filepath = os.path.join(autotest, "locations.txt")
    swarm_init_filepath = os.path.join(autotest, "locations.txt")
    comment_regex = re.compile("\s*#.*")
    for path in [locations_userpath, locations_filepath]:
        if not os.path.isfile(path):
            continue

        with open(path, 'r') as fd:
            for line in fd:
                line = re.sub(comment_regex, "", line)
                line = line.rstrip("\n")
                if len(line) == 0:
                    continue
                (name, loc) = line.split("=")
                if name == locname:
                    if cmd_opts.swarm :
                        (lat, lon, alt, heading)=loc.split(",")
                        for path2 in [swarm_init_filepath]:
                            if os.path.isfile(path2):
                                with open(path2,'r') as swd:
                                    for lines in swd:
                                        if len(lines) == 0:
                                            continue
                                        (instance, offset) = lines.split("=")
                                        if ((int)(instance) == (int)cmd_opts.instance):
                                            (dist, heading) = offset.split(",")
                                            g=mp_util.gps_newpos((float)(lat), (float)(lon), (float)(heading), (float)(dist))
                                            loc=str(g[0])+","+str(g[1])+","+str(alt)+","+str(heading)
                                            return loc
                                g=mp_util.gps_newpos((float)(lat), (float)(lon), 90, 20*(int)(cmd_opts.instance))    
                                loc=str(g[0])+","+str(g[1])+","+str(alt)+","+str(heading)
                                return loc
                    return loc
Exemple #18
0
def mavlink_packet(m):
    '''handle an incoming mavlink packet'''
    state = mpstate.map_state

    if m.get_type() == "SIMSTATE":
        if not mpstate.map_state.have_simstate:
            mpstate.map_state.have_simstate  = True
            create_blueplane()
        mpstate.map.set_position('blueplane', (m.lat, m.lng), rotation=math.degrees(m.yaw))

    if m.get_type() == "GPS_RAW_INT" and not mpstate.map_state.have_simstate:
        (lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
        if state.lat is not None and (mpstate.map_state.have_blueplane or
                                      mp_util.gps_distance(lat, lon, state.lat, state.lon) > 10):
            create_blueplane()
            mpstate.map.set_position('blueplane', (lat, lon), rotation=m.cog*0.01)

    if m.get_type() == "NAV_CONTROLLER_OUTPUT":
        if mpstate.master().flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL" ]:
            trajectory = [ (state.lat, state.lon),
                           mp_util.gps_newpos(state.lat, state.lon, m.target_bearing, m.wp_dist) ]
            mpstate.map.add_object(mp_slipmap.SlipPolygon('trajectory', trajectory, layer='Trajectory',
                                                          linewidth=2, colour=(255,0,180)))
        else:
            mpstate.map.add_object(mp_slipmap.SlipClearLayer('Trajectory'))

        
    if m.get_type() == 'GLOBAL_POSITION_INT':
        (state.lat, state.lon, state.heading) = (m.lat*1.0e-7, m.lon*1.0e-7, m.hdg*0.01)
    else:
        return

    if state.lat != 0 or state.lon != 0:
        mpstate.map.set_position('plane', (state.lat, state.lon), rotation=state.heading)

    # if the waypoints have changed, redisplay
    if state.wp_change_time != mpstate.status.wploader.last_change:
        state.wp_change_time = mpstate.status.wploader.last_change
        display_waypoints()

    # if the fence has changed, redisplay
    if state.fence_change_time != mpstate.status.fenceloader.last_change:
        state.fence_change_time = mpstate.status.fenceloader.last_change
        points = mpstate.status.fenceloader.polygon()
        if len(points) > 1:
            mpstate.map.add_object(mp_slipmap.SlipPolygon('fence', points, layer=1, linewidth=2, colour=(0,255,0)))

    # check for any events from the map
    mpstate.map.check_events()
    def mavlink_packet_distance_sensor(self, vehicle, m):
        heading = self.heading_for_vehicle.get(vehicle, 0)
        tlayer = "Distance sensor for %u.%u id=%u" % (
            m.get_srcSystem(), m.get_srcComponent(), m.id)
        slipkey = '%s-POS%u' % (tlayer, m.orientation)
        if not m.get_srcSystem() in self.lat_lon_for_vehicle:
            return
        if m.current_distance == m.max_distance:
            self.foreach_map(lambda a_map : a_map.remove_object(slipkey))
            return

        (lat,lon) = self.lat_lon_for_vehicle[m.get_srcSystem()]
        mav_sensor_rotation_to_degrees = {
            mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 0,
            mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 45,
            mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 90,
            mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135 : 135,
            mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 180,
            mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225 : 225,
            mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270 : 270,
            mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315 : 315,
        }
        if m.orientation in mav_sensor_rotation_to_degrees:
            degrees = mav_sensor_rotation_to_degrees[m.orientation]
        else:
#            print("bad orientation (%u)" % m.orientation)
            return
        if m.current_distance >= m.max_distance:
            return

        p = mp_util.gps_newpos(lat, lon, heading+degrees, 0)
        # start angle/end angle are either side of the primary axis,
        # which is rotated to be North
        start_angle = -22.5
        end_angle = 22.5
        self.foreach_map(lambda a_map :
                         a_map.add_object(mp_slipmap.SlipCircle(
                             slipkey,
                             3,
                             p,
                             m.current_distance/100.0,
                             (255, 127, 0),
                             linewidth=3,
                             start_angle=start_angle,
                             end_angle=end_angle,
                             rotation=(-90+(heading+degrees))%360,
                         ))
        )
Exemple #20
0
def find_new_spawn(loc, file_path):
    (lat, lon, alt, heading)=loc.split(",")
    swarminit_filepath = os.path.join(autotest, "swarminit.txt")
    for path2 in [file_path, swarminit_filepath]:
        if os.path.isfile(path2):
            with open(path2,'r') as swd:
                next(swd)
                for lines in swd:
                    if len(lines) == 0:
                        continue
                    (instance, offset) = lines.split("=")
                    if ((int)(instance) == (int)(cmd_opts.instance)):
                        (x,y,z,head) = offset.split(",")
                        g=mp_util.gps_offset((float)(lat), (float)(lon), (float)(x), (float)(y))
                        loc=str(g[0])+","+str(g[1])+","+str(alt+z)+","+str(head)
                        return loc
        g=mp_util.gps_newpos((float)(lat), (float)(lon), 90, 20*(int)(cmd_opts.instance))    
        loc=str(g[0])+","+str(g[1])+","+str(alt)+","+str(heading)
        return loc
Exemple #21
0
def find_new_spawn(loc, file_path):
    (lat, lon, alt, heading) = loc.split(",")
    swarminit_filepath = os.path.join(find_autotest_dir(), "swarminit.txt")
    for path2 in [file_path, swarminit_filepath]:
        if os.path.isfile(path2):
            with open(path2, 'r') as swd:
                next(swd)
                for lines in swd:
                    if len(lines) == 0:
                        continue
                    (instance, offset) = lines.split("=")
                    if ((int)(instance) == (int)(cmd_opts.instance)):
                        (x, y, z, head) = offset.split(",")
                        g = mp_util.gps_offset((float)(lat), (float)(lon), (float)(x), (float)(y))
                        loc = str(g[0])+","+str(g[1])+","+str(alt+z)+","+str(head)
                        return loc
        g = mp_util.gps_newpos((float)(lat), (float)(lon), 90, 20*(int)(cmd_opts.instance))
        loc = str(g[0])+","+str(g[1])+","+str(alt)+","+str(heading)
        return loc
Exemple #22
0
def mavflightview(filename):
    print("Loading %s ..." % filename)
    mlog = mavutil.mavlink_connection(filename)
    wp = mavwp.MAVWPLoader()
    if opts.mission is not None:
        wp.load(opts.mission)
    fen = mavwp.MAVFenceLoader()
    if opts.fence is not None:
        fen.load(opts.fence)
    path = [[]]
    types = ['MISSION_ITEM']
    if opts.rawgps:
        types.extend(['GPS', 'GPS_RAW_INT'])
    if opts.rawgps2:
        types.extend(['GPS2_RAW','GPS2'])
    if opts.dualgps:
        types.extend(['GPS2_RAW','GPS2', 'GPS_RAW_INT', 'GPS'])
    if opts.ekf:
        types.extend(['EKF1'])
    if len(types) == 1:
        types.extend(['GPS','GLOBAL_POSITION_INT'])
    print("Looking for types %s" % str(types))
    while True:
        try:
            m = mlog.recv_match(type=types)
            if m is None:
                break
        except Exception:
            break
        if m.get_type() == 'MISSION_ITEM':
            wp.set(m, m.seq)            
            continue
        if not mlog.check_condition(opts.condition):
            continue
        if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
            continue
        if m.get_type() in ['GPS','GPS2']:
            status = getattr(m, 'Status', None)
            if status is None:
                status = getattr(m, 'FixType', None)
                if status is None:
                    print("Can't find status on GPS message")
                    print(m)
                    break
            if status < 2:
                continue
            # flash log
            lat = m.Lat
            lng = getattr(m, 'Lng', None)
            if lng is None:
                lng = getattr(m, 'Lon', None)
                if lng is None:
                    print("Can't find longitude on GPS message")
                    print(m)
                    break                    
        elif m.get_type() == 'EKF1':
            pos = mavextra.ekf1_pos(m)
            if pos is None:
                continue
            (lat, lng) = pos            
        else:
            lat = m.lat * 1.0e-7
            lng = m.lon * 1.0e-7
        instance = 0
        if opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']:
            instance = 1
        if m.get_type() == 'EKF1':
            if opts.dualgps:
                instance = 2
            else:
                instance = 1
        if lat != 0 or lng != 0:
            if getattr(mlog, 'flightmode','') in colourmap:
                colour = colourmap[mlog.flightmode]
                (r,g,b) = colour
                (r,g,b) = (r+instance*50,g+instance*50,b+instance*50)
                if r > 255:
                    r = 205
                if g > 255:
                    g = 205
                if b > 255:
                    b = 205
                colour = (r,g,b)
                point = (lat, lng, colour)
            else:
                point = (lat, lng)
            while instance >= len(path):
                path.append([])
            path[instance].append(point)
    if len(path[0]) == 0:
        print("No points to plot")
        return
    bounds = mp_util.polygon_bounds(path[0])
    (lat, lon) = (bounds[0]+bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
    while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
           mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
        ground_width += 10

    path_objs = []
    for i in range(len(path)):
        if len(path[i]) != 0:
            path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,filename), path[i], layer='FlightPath',
                                                    linewidth=2, colour=(255,0,180)))
    mission = wp.polygon()
    if len(mission) > 1:
        mission_obj = mp_slipmap.SlipPolygon('Mission-%s' % filename, wp.polygon(), layer='Mission',
                                             linewidth=2, colour=(255,255,255))
    else:
        mission_obj = None

    fence = fen.polygon()
    if len(fence) > 1:
        fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename, fen.polygon(), layer='Fence',
                                           linewidth=2, colour=(0,255,0))
    else:
        fence_obj = None

    if opts.imagefile:
        create_imagefile(opts.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
    else:
        global multi_map
        if opts.multi and multi_map is not None:
            map = multi_map
        else:
            map = mp_slipmap.MPSlipMap(title=filename,
                                       service=opts.service,
                                       elevation=True,
                                       width=600,
                                       height=600,
                                       ground_width=ground_width,
                                       lat=lat, lon=lon,
                                       debug=opts.debug)
        if opts.multi:
            multi_map = map
        for path_obj in path_objs:
            map.add_object(path_obj)
        if mission_obj is not None:
            map.add_object(mission_obj)
        if fence_obj is not None:
            map.add_object(fence_obj)

        for flag in opts.flag:
            a = flag.split(',')
            lat = a[0]
            lon = a[1]
            icon = 'flag.png'
            if len(a) > 2:
                icon = a[2] + '.png'
            icon = map.icon(icon)
            map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
Exemple #23
0
def mavflightview(filename):
    print("Loading %s ..." % filename)
    mlog = mavutil.mavlink_connection(filename)
    wp = mavwp.MAVWPLoader()
    if opts.mission is not None:
        wp.load(opts.mission)
    path = []
    path2 = []
    types = ['MISSION_ITEM']
    if opts.rawgps:
        types.extend(['GPS', 'GPS_RAW_INT'])
    if opts.rawgps2:
        types.extend(['GPS2_RAW', 'GPS2'])
    if opts.dualgps:
        types.extend(['GPS2_RAW', 'GPS2', 'GPS_RAW_INT', 'GPS'])
    if len(types) == 1:
        types.extend(['GPS', 'GLOBAL_POSITION_INT'])
    while True:
        m = mlog.recv_match(type=types)
        if m is None:
            break
        if m.get_type() == 'MISSION_ITEM':
            wp.set(m, m.seq)
            continue
        if not mlog.check_condition(opts.condition):
            continue
        if opts.mode is not None and mlog.flightmode.lower(
        ) != opts.mode.lower():
            continue
        if m.get_type() in ['GPS', 'GPS2']:
            status = getattr(m, 'Status', None)
            if status is None:
                status = getattr(m, 'FixType', None)
                if status is None:
                    print("Can't find status on GPS message")
                    print(m)
                    break
            if status < 2:
                continue
            # flash log
            lat = m.Lat
            lng = getattr(m, 'Lng', None)
            if lng is None:
                lng = getattr(m, 'Lon', None)
                if lng is None:
                    print("Can't find longitude on GPS message")
                    print(m)
                    break
        else:
            lat = m.lat * 1.0e-7
            lng = m.lon * 1.0e-7
        secondary = opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']
        if lat != 0 or lng != 0:
            if getattr(mlog, 'flightmode', '') in colourmap:
                colour = colourmap[mlog.flightmode]
                if secondary:
                    (r, g, b) = colour
                    (r, g, b) = (r + 50, g + 50, b + 50)
                    if r > 255:
                        r = 205
                    if g > 255:
                        g = 205
                    if b > 255:
                        b = 205
                    colour = (r, g, b)
                point = (lat, lng, colour)
            else:
                point = (lat, lng)
            if secondary:
                path2.append(point)
            else:
                path.append(point)
    if len(path) == 0:
        print("No points to plot")
        return
    bounds = mp_util.polygon_bounds(path)
    (lat, lon) = (bounds[0] + bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
                                        lon + bounds[3])
    while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
           >= ground_width - 20 or mp_util.gps_distance(
               lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
        ground_width += 10

    path_obj = mp_slipmap.SlipPolygon('FlightPath-%s' % filename,
                                      path,
                                      layer='FlightPath',
                                      linewidth=2,
                                      colour=(255, 0, 180))
    if len(path2) != 0:
        path2_obj = mp_slipmap.SlipPolygon('FlightPath2-%s' % filename,
                                           path2,
                                           layer='FlightPath',
                                           linewidth=2,
                                           colour=(255, 0, 180))
    else:
        path2_obj = None
    mission = wp.polygon()
    if len(mission) > 1:
        mission_obj = mp_slipmap.SlipPolygon('Mission-%s' % filename,
                                             wp.polygon(),
                                             layer='Mission',
                                             linewidth=2,
                                             colour=(255, 255, 255))
    else:
        mission_obj = None

    if opts.imagefile:
        create_imagefile(opts.imagefile, (lat, lon),
                         ground_width,
                         path_obj,
                         mission_obj,
                         path2_obj=path2_obj)
    else:
        global multi_map
        if opts.multi and multi_map is not None:
            map = multi_map
        else:
            map = mp_slipmap.MPSlipMap(title=filename,
                                       service=opts.service,
                                       elevation=True,
                                       width=600,
                                       height=600,
                                       ground_width=ground_width,
                                       lat=lat,
                                       lon=lon,
                                       debug=opts.debug)
        if opts.multi:
            multi_map = map
        map.add_object(path_obj)
        if path2_obj is not None:
            map.add_object(path2_obj)
        if mission_obj is not None:
            map.add_object(mission_obj)

        for flag in opts.flag:
            a = flag.split(',')
            lat = a[0]
            lon = a[1]
            icon = 'flag.png'
            if len(a) > 2:
                icon = a[2] + '.png'
            icon = map.icon(icon)
            map.add_object(
                mp_slipmap.SlipIcon('icon - %s' % str(flag),
                                    (float(lat), float(lon)),
                                    icon,
                                    layer=3,
                                    rotation=0,
                                    follow=False))
 def move(self, bearing, distance):
     '''move position by bearing and distance'''
     lat = self.pkt['I105']['Lat']['val']
     lon = self.pkt['I105']['Lon']['val']
     (lat, lon) = mp_util.gps_newpos(lat, lon, bearing, distance)
     self.setpos(lat, lon)
def mavflightview_show(path, wp, fen, options, title=None):
    if not title:
        title='MAVFlightView'

    bounds = mp_util.polygon_bounds(path[0])
    (lat, lon) = (bounds[0]+bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
    while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
           mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
        ground_width += 10

    path_objs = []
    for i in range(len(path)):
        if len(path[i]) != 0:
            path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,title), path[i], layer='FlightPath',
                                                    linewidth=2, colour=(255,0,180)))
    plist = wp.polygon_list()
    mission_obj = None
    if len(plist) > 0:
        mission_obj = []
        for i in range(len(plist)):
            mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (title,i), plist[i], layer='Mission',
                                                      linewidth=2, colour=(255,255,255)))
    else:
        mission_obj = None

    fence = fen.polygon()
    if len(fence) > 1:
        fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence',
                                           linewidth=2, colour=(0,255,0))
    else:
        fence_obj = None

    if options.imagefile:
        create_imagefile(options, options.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
    else:
        global multi_map
        if options.multi and multi_map is not None:
            map = multi_map
        else:
            map = mp_slipmap.MPSlipMap(title=title,
                                       service=options.service,
                                       elevation=True,
                                       width=600,
                                       height=600,
                                       ground_width=ground_width,
                                       lat=lat, lon=lon,
                                       debug=options.debug)
        if options.multi:
            multi_map = map
        for path_obj in path_objs:
            map.add_object(path_obj)
        if mission_obj is not None:
            display_waypoints(wp, map)
        if fence_obj is not None:
            map.add_object(fence_obj)

        for flag in options.flag:
            a = flag.split(',')
            lat = a[0]
            lon = a[1]
            icon = 'flag.png'
            if len(a) > 2:
                icon = a[2] + '.png'
            icon = map.icon(icon)
            map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))

    source = getattr(options, "colour_source", "flightmode")
    if source != "flightmode":
        print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
Exemple #26
0
def mavlink_packet(m):
    '''handle an incoming mavlink packet'''
    state = mpstate.map_state

    if m.get_type() == "HEARTBEAT":
        from pymavlink import mavutil
        if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
            state.vehicle_type = 'plane'
        elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
                        mavutil.mavlink.MAV_TYPE_SURFACE_BOAT,
                        mavutil.mavlink.MAV_TYPE_SUBMARINE]:
            state.vehicle_type = 'rover'
        elif m.type in [mavutil.mavlink.MAV_TYPE_QUADROTOR,
                        mavutil.mavlink.MAV_TYPE_COAXIAL,
                        mavutil.mavlink.MAV_TYPE_HEXAROTOR,
                        mavutil.mavlink.MAV_TYPE_OCTOROTOR,
                        mavutil.mavlink.MAV_TYPE_TRICOPTER,
                        mavutil.mavlink.MAV_TYPE_HELICOPTER]:
            state.vehicle_type = 'copter'

    if m.get_type() == "SIMSTATE" and state.settings.showsimpos:
        create_vehicle_icon('SimVehicle', 'green')
        mpstate.map.set_position('SimVehicle', (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))

    if m.get_type() == "AHRS2" and state.settings.showahrs2pos:
        create_vehicle_icon('AHRS2Vehicle', 'blue')
        mpstate.map.set_position('AHRS2Vehicle', (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))

    if m.get_type() == "GPS_RAW_INT" and state.settings.showgpspos:
        (lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
        if lat != 0 or lon != 0:
            create_vehicle_icon('GPSVehicle', 'blue')
            mpstate.map.set_position('GPSVehicle', (lat, lon), rotation=m.cog*0.01)

    if m.get_type() == "GPS2_RAW" and state.settings.showgps2pos:
        (lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
        if lat != 0 or lon != 0:
            create_vehicle_icon('GPS2Vehicle', 'green')
            mpstate.map.set_position('GPS2Vehicle', (lat, lon), rotation=m.cog*0.01)

    if m.get_type() == 'GLOBAL_POSITION_INT':
        (state.lat, state.lon, state.heading) = (m.lat*1.0e-7, m.lon*1.0e-7, m.hdg*0.01)
        if state.lat != 0 or state.lon != 0:
            create_vehicle_icon('PosVehicle', 'red', follow=True)
            mpstate.map.set_position('PosVehicle', (state.lat, state.lon), rotation=state.heading)

    if m.get_type() == "NAV_CONTROLLER_OUTPUT":
        if mpstate.master().flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL" ]:
            trajectory = [ (state.lat, state.lon),
                           mp_util.gps_newpos(state.lat, state.lon, m.target_bearing, m.wp_dist) ]
            mpstate.map.add_object(mp_slipmap.SlipPolygon('trajectory', trajectory, layer='Trajectory',
                                                          linewidth=2, colour=(255,0,180)))
        else:
            mpstate.map.add_object(mp_slipmap.SlipClearLayer('Trajectory'))


    # if the waypoints have changed, redisplay
    if state.wp_change_time != mpstate.status.wploader.last_change:
        state.wp_change_time = mpstate.status.wploader.last_change
        display_waypoints()

    # if the fence has changed, redisplay
    if state.fence_change_time != mpstate.fence.fenceloader.last_change:
        state.fence_change_time = mpstate.fence.fenceloader.last_change
        points = mpstate.fence.fenceloader.polygon()
        if len(points) > 1:
            mpstate.map.add_object(mp_slipmap.SlipPolygon('fence', points, layer=1, linewidth=2, colour=(0,255,0)))

    # if the rallypoints have changed, redisplay
    if state.rally_change_time != mpstate.rally_state.rallyloader.last_change:
        state.rally_change_time = mpstate.rally_state.rallyloader.last_change
        icon = mpstate.map.icon('rallypoint.png')
        mpstate.map.add_object(mp_slipmap.SlipClearLayer('RallyPoints'))
        for i in range(mpstate.rally_state.rallyloader.rally_count()):
            rp = mpstate.rally_state.rallyloader.rally_point(i)
            mpstate.map.add_object(mp_slipmap.SlipIcon('Rally-%u' % i, (rp.lat*1.0e-7, rp.lng*1.0e-7), icon,
                                                       layer='RallyPoints', rotation=0, follow=False))

    # check for any events from the map
    mpstate.map.check_events()
Exemple #27
0
    def mavlink_packet(self, m):
        '''handle an incoming mavlink packet'''
        from MAVProxy.modules.mavproxy_map import mp_slipmap
        if m.get_type() == "HEARTBEAT":
            if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
                self.vehicle_type_name = 'plane'
            elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
                            mavutil.mavlink.MAV_TYPE_SURFACE_BOAT,
                            mavutil.mavlink.MAV_TYPE_SUBMARINE]:
                self.vehicle_type_name = 'rover'
            elif m.type in [mavutil.mavlink.MAV_TYPE_QUADROTOR,
                            mavutil.mavlink.MAV_TYPE_COAXIAL,
                            mavutil.mavlink.MAV_TYPE_HEXAROTOR,
                            mavutil.mavlink.MAV_TYPE_OCTOROTOR,
                            mavutil.mavlink.MAV_TYPE_TRICOPTER]:
                self.vehicle_type_name = 'copter'
            elif m.type in [mavutil.mavlink.MAV_TYPE_HELICOPTER]:
                self.vehicle_type_name = 'heli'
            elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
                self.vehicle_type_name = 'antenna'     
    
        # this is the beginnings of allowing support for multiple vehicles
        # in the air at the same time
        vehicle = 'Vehicle%u' % m.get_srcSystem()
    
        if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos:
            self.create_vehicle_icon('Sim' + vehicle, 'green')
            self.mpstate.map.set_position('Sim' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))
    
        if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos:
            self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
            self.mpstate.map.set_position('AHRS2' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))

        if m.get_type() == "AHRS3" and self.map_settings.showahrs3pos:
            self.create_vehicle_icon('AHRS3' + vehicle, 'orange')
            self.mpstate.map.set_position('AHRS3' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))

        if m.get_type() == "GPS_RAW_INT" and self.map_settings.showgpspos:
            (lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
            if lat != 0 or lon != 0:
                self.create_vehicle_icon('GPS' + vehicle, 'blue')
                self.mpstate.map.set_position('GPS' + vehicle, (lat, lon), rotation=m.cog*0.01)
    
        if m.get_type() == "GPS2_RAW" and self.map_settings.showgps2pos:
            (lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
            if lat != 0 or lon != 0:
                self.create_vehicle_icon('GPS2' + vehicle, 'green')
                self.mpstate.map.set_position('GPS2' + vehicle, (lat, lon), rotation=m.cog*0.01)
    
        if m.get_type() == 'GLOBAL_POSITION_INT':
            (self.lat, self.lon, self.heading) = (m.lat*1.0e-7, m.lon*1.0e-7, m.hdg*0.01)
            if abs(self.lat) > 1.0e-3 or abs(self.lon) > 1.0e-3:
                self.have_global_position = True
                self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
                self.mpstate.map.set_position('Pos' + vehicle, (self.lat, self.lon), rotation=self.heading)

        if m.get_type() == 'LOCAL_POSITION_NED' and not self.have_global_position:
            (self.lat, self.lon) = mp_util.gps_offset(0, 0, m.x, m.y)
            self.heading = math.degrees(math.atan2(m.vy, m.vx))
            self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
            self.mpstate.map.set_position('Pos' + vehicle, (self.lat, self.lon), rotation=self.heading)
    
        if m.get_type() == "NAV_CONTROLLER_OUTPUT":
            if (self.master.flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL" ] and
                self.lat is not None and self.lon is not None):
                trajectory = [ (self.lat, self.lon),
                               mp_util.gps_newpos(self.lat, self.lon, m.target_bearing, m.wp_dist) ]
                self.mpstate.map.add_object(mp_slipmap.SlipPolygon('trajectory', trajectory, layer='Trajectory',
                                                                   linewidth=2, colour=(255,0,180)))
            else:
                self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Trajectory'))    
            
        # if the waypoints have changed, redisplay
        last_wp_change = self.module('wp').wploader.last_change
        if self.wp_change_time != last_wp_change and abs(time.time() - last_wp_change) > 1:
            self.wp_change_time = last_wp_change
            self.display_waypoints()

            #this may have affected the landing lines from the rally points:
            self.rally_change_time = time.time()
    
        # if the fence has changed, redisplay
        if self.fence_change_time != self.module('fence').fenceloader.last_change:
            self.display_fence()
    
        # if the rallypoints have changed, redisplay
        if self.rally_change_time != self.module('rally').rallyloader.last_change:
            self.rally_change_time = self.module('rally').rallyloader.last_change
            icon = self.mpstate.map.icon('rallypoint.png')
            self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('RallyPoints'))
            for i in range(self.module('rally').rallyloader.rally_count()):
                rp = self.module('rally').rallyloader.rally_point(i)
                popup = MPMenuSubMenu('Popup',
                                      items=[MPMenuItem('Rally Remove', returnkey='popupRallyRemove'),
                                             MPMenuItem('Rally Move', returnkey='popupRallyMove')])
                self.mpstate.map.add_object(mp_slipmap.SlipIcon('Rally %u' % (i+1), (rp.lat*1.0e-7, rp.lng*1.0e-7), icon,
                                                                layer='RallyPoints', rotation=0, follow=False,
                                                                popup_menu=popup))

                loiter_rad = self.get_mav_param('WP_LOITER_RAD')

                if self.map_settings.rallycircle:
                    self.mpstate.map.add_object(mp_slipmap.SlipCircle('Rally Circ %u' % (i+1), 'RallyPoints', (rp.lat*1.0e-7, rp.lng*1.0e-7), abs(loiter_rad), (255,255,0), 2))

                #draw a line between rally point and nearest landing point
                nearest_land_wp = None
                nearest_distance = 10000000.0
                for j in range(self.module('wp').wploader.count()):
                    w = self.module('wp').wploader.wp(j)
                    if (w.command == 21): #if landing waypoint
                        #get distance between rally point and this waypoint
                        dis = mp_util.gps_distance(w.x, w.y, rp.lat*1.0e-7, rp.lng*1.0e-7)
                        if (dis < nearest_distance):
                            nearest_land_wp = w
                            nearest_distance = dis

                if nearest_land_wp != None:
                    points = []
                    #tangential approach?
                    if self.get_mav_param('LAND_BREAK_PATH') == 0:
                        theta = math.degrees(math.atan(loiter_rad / nearest_distance))
                        tan_dis = math.sqrt(nearest_distance * nearest_distance - (loiter_rad * loiter_rad))

                        ral_bearing = mp_util.gps_bearing(nearest_land_wp.x, nearest_land_wp.y,rp.lat*1.0e-7, rp.lng*1.0e-7)

                        points.append(mp_util.gps_newpos(nearest_land_wp.x,nearest_land_wp.y, ral_bearing + theta, tan_dis))

                    else: #not tangential approach
                        points.append((rp.lat*1.0e-7, rp.lng*1.0e-7))

                    points.append((nearest_land_wp.x, nearest_land_wp.y))
                    self.mpstate.map.add_object(mp_slipmap.SlipPolygon('Rally Land %u' % (i+1), points, 'RallyPoints', (255,255,0), 2))

        # check for any events from the map
        self.mpstate.map.check_events()
Exemple #28
0
            wp = mavwp.MAVWPLoader()
            wp.load(file)
            boundary = wp.polygon()
            sm.add_object(SlipPolygon('mission-%s' % file, boundary, layer=1, linewidth=1, colour=(255,255,255)))

    if opts.grid:
        sm.add_object(SlipGrid('grid', layer=3, linewidth=1, colour=(255,255,0)))

    if opts.thumbnail:
        thumb = cv.LoadImage(opts.thumbnail)
        sm.add_object(SlipThumbnail('thumb', (opts.lat,opts.lon), layer=1, img=thumb, border_width=2, border_colour=(255,0,0)))

    if opts.icon:
        icon = cv.LoadImage(opts.icon)
        sm.add_object(SlipIcon('icon', (opts.lat,opts.lon), icon, layer=3, rotation=90, follow=True))
        sm.set_position('icon', mp_util.gps_newpos(opts.lat,opts.lon, 180, 100), rotation=45)
        sm.add_object(SlipInfoImage('detail', icon))
        sm.add_object(SlipInfoText('detail text', 'test text'))

    for flag in opts.flag:
        (lat,lon) = flag.split(',')
        icon = sm.icon('flag.png')
        sm.add_object(SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))

    while sm.is_alive():
        while sm.event_count() > 0:
            obj = sm.get_event()
            if not opts.verbose:
                continue
            if isinstance(obj, SlipMouseEvent):
                print("Mouse event at %s (X/Y=%u/%u) for %u objects" % (obj.latlon,
Exemple #29
0
    def mavlink_packet(self, m):
        '''handle an incoming mavlink packet'''
        from MAVProxy.modules.mavproxy_map import mp_slipmap
        mtype = m.get_type()
        sysid = m.get_srcSystem()

        if mtype == "HEARTBEAT":
            vname = 'plane'
            if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
                vname = 'plane'
            elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER]:
                vname = 'rover'
            elif m.type in [mavutil.mavlink.MAV_TYPE_SUBMARINE]:
                vname = 'sub'
            elif m.type in [mavutil.mavlink.MAV_TYPE_SURFACE_BOAT]:
                vname = 'boat'
            elif m.type in [
                    mavutil.mavlink.MAV_TYPE_QUADROTOR,
                    mavutil.mavlink.MAV_TYPE_HEXAROTOR,
                    mavutil.mavlink.MAV_TYPE_OCTOROTOR,
                    mavutil.mavlink.MAV_TYPE_TRICOPTER
            ]:
                vname = 'copter'
            elif m.type in [mavutil.mavlink.MAV_TYPE_COAXIAL]:
                vname = 'singlecopter'
            elif m.type in [mavutil.mavlink.MAV_TYPE_HELICOPTER]:
                vname = 'heli'
            elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
                vname = 'antenna'
            self.vehicle_type_by_sysid[sysid] = vname

        if not sysid in self.vehicle_type_by_sysid:
            self.vehicle_type_by_sysid[sysid] = 'plane'
        self.vehicle_type_name = self.vehicle_type_by_sysid[sysid]

        # this is the beginnings of allowing support for multiple vehicles
        # in the air at the same time
        vehicle = 'Vehicle%u' % m.get_srcSystem()

        if mtype == "SIMSTATE" and self.map_settings.showsimpos:
            self.create_vehicle_icon('Sim' + vehicle, 'green')
            self.map.set_position('Sim' + vehicle,
                                  (m.lat * 1.0e-7, m.lng * 1.0e-7),
                                  rotation=math.degrees(m.yaw))

        elif mtype == "AHRS2" and self.map_settings.showahrs2pos:
            self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
            self.map.set_position('AHRS2' + vehicle,
                                  (m.lat * 1.0e-7, m.lng * 1.0e-7),
                                  rotation=math.degrees(m.yaw))

        elif mtype == "AHRS3" and self.map_settings.showahrs3pos:
            self.create_vehicle_icon('AHRS3' + vehicle, 'orange')
            self.map.set_position('AHRS3' + vehicle,
                                  (m.lat * 1.0e-7, m.lng * 1.0e-7),
                                  rotation=math.degrees(m.yaw))

        elif mtype == "GPS_RAW_INT" and self.map_settings.showgpspos:
            (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
            if lat != 0 or lon != 0:
                if m.vel > 300 or 'ATTITUDE' not in self.master.messages:
                    cog = m.cog * 0.01
                else:
                    cog = math.degrees(self.master.messages['ATTITUDE'].yaw)
                self.create_vehicle_icon('GPS' + vehicle, 'blue')
                self.map.set_position('GPS' + vehicle, (lat, lon),
                                      rotation=cog)

        elif mtype == "GPS2_RAW" and self.map_settings.showgps2pos:
            (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
            if lat != 0 or lon != 0:
                self.create_vehicle_icon('GPS2' + vehicle, 'green')
                self.map.set_position('GPS2' + vehicle, (lat, lon),
                                      rotation=m.cog * 0.01)

        elif mtype == 'GLOBAL_POSITION_INT' and self.map_settings.showahrspos:
            (lat, lon, heading) = (m.lat * 1.0e-7, m.lon * 1.0e-7,
                                   m.hdg * 0.01)
            self.lat_lon[m.get_srcSystem()] = (lat, lon)
            if abs(lat) > 1.0e-3 or abs(lon) > 1.0e-3:
                self.have_global_position = True
                self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
                if len(self.vehicle_type_by_sysid) > 1:
                    label = str(sysid)
                else:
                    label = None
                self.map.set_position('Pos' + vehicle, (lat, lon),
                                      rotation=heading,
                                      label=label,
                                      colour=(255, 255, 255))
                self.map.set_follow_object('Pos' + vehicle,
                                           self.is_primary_vehicle(m))

        elif mtype == 'HOME_POSITION':
            (lat, lon) = (m.latitude * 1.0e-7, m.longitude * 1.0e-7)
            icon = self.map.icon('home.png')
            self.map.add_object(
                mp_slipmap.SlipIcon('HOME_POSITION', (lat, lon),
                                    icon,
                                    layer=3,
                                    rotation=0,
                                    follow=False))

        elif mtype == "NAV_CONTROLLER_OUTPUT":
            tlayer = 'Trajectory%u' % m.get_srcSystem()
            if (self.master.flightmode in [
                    "AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER",
                    "QLAND", "FOLLOW"
            ] and m.get_srcSystem() in self.lat_lon):
                (lat, lon) = self.lat_lon[m.get_srcSystem()]
                trajectory = [(lat, lon),
                              mp_util.gps_newpos(lat, lon, m.target_bearing,
                                                 m.wp_dist)]
                self.map.add_object(
                    mp_slipmap.SlipPolygon('trajectory',
                                           trajectory,
                                           layer=tlayer,
                                           linewidth=2,
                                           colour=(255, 0, 180)))
                self.trajectory_layers.add(tlayer)
            else:
                if tlayer in self.trajectory_layers:
                    self.map.add_object(mp_slipmap.SlipClearLayer(tlayer))
                    self.trajectory_layers.remove(tlayer)

        elif mtype == "POSITION_TARGET_GLOBAL_INT":
            # FIXME: base this off SYS_STATUS.MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL?
            if not m.get_srcSystem() in self.lat_lon:
                return
            tlayer = 'PostionTarget%u' % m.get_srcSystem()
            (lat, lon) = self.lat_lon[m.get_srcSystem()]
            if (self.master.flightmode in [
                    "AUTO", "GUIDED", "LOITER", "RTL", "QRTL", "QLOITER",
                    "QLAND", "FOLLOW"
            ]):
                lat_float = m.lat_int * 1e-7
                lon_float = m.lon_int * 1e-7
                vec = [(lat_float, lon_float), (lat, lon)]
                self.map.add_object(
                    mp_slipmap.SlipPolygon('position_target',
                                           vec,
                                           layer=tlayer,
                                           linewidth=2,
                                           colour=(0, 255, 0)))
            else:
                self.map.add_object(mp_slipmap.SlipClearLayer(tlayer))

        if not self.is_primary_vehicle(m):
            # the rest should only be done for the primary vehicle
            return

        # if the waypoints have changed, redisplay
        last_wp_change = self.module('wp').wploader.last_change
        if self.wp_change_time != last_wp_change and abs(time.time() -
                                                         last_wp_change) > 1:
            self.wp_change_time = last_wp_change
            self.display_waypoints()

            #this may have affected the landing lines from the rally points:
            self.rally_change_time = time.time()

        # if the fence has changed, redisplay
        if (self.module('fence') and self.fence_change_time !=
                self.module('fence').fenceloader.last_change):
            self.display_fence()

        # if the rallypoints have changed, redisplay
        if (self.module('rally') and
                self.rally_change_time != self.module('rally').last_change()):
            self.rally_change_time = self.module('rally').last_change()
            icon = self.map.icon('rallypoint.png')
            self.map.add_object(mp_slipmap.SlipClearLayer('RallyPoints'))
            for i in range(self.module('rally').rally_count()):
                rp = self.module('rally').rally_point(i)
                popup = MPMenuSubMenu(
                    'Popup',
                    items=[
                        MPMenuItem('Rally Remove',
                                   returnkey='popupRallyRemove'),
                        MPMenuItem('Rally Move', returnkey='popupRallyMove')
                    ])
                self.map.add_object(
                    mp_slipmap.SlipIcon('Rally %u' % (i + 1),
                                        (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
                                        icon,
                                        layer='RallyPoints',
                                        rotation=0,
                                        follow=False,
                                        popup_menu=popup))

                loiter_rad = self.get_mav_param('WP_LOITER_RAD')

                if self.map_settings.rallycircle:
                    self.map.add_object(
                        mp_slipmap.SlipCircle(
                            'Rally Circ %u' % (i + 1),
                            'RallyPoints', (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
                            loiter_rad, (255, 255, 0),
                            2,
                            arrow=self.map_settings.showdirection))

                #draw a line between rally point and nearest landing point
                nearest_land_wp = None
                nearest_distance = 10000000.0
                for j in range(self.module('wp').wploader.count()):
                    w = self.module('wp').wploader.wp(j)
                    if (w.command == 21):  #if landing waypoint
                        #get distance between rally point and this waypoint
                        dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7,
                                                   rp.lng * 1.0e-7)
                        if (dis < nearest_distance):
                            nearest_land_wp = w
                            nearest_distance = dis

                if nearest_land_wp is not None:
                    points = []
                    #tangential approach?
                    if self.get_mav_param('LAND_BREAK_PATH') == 0:
                        theta = math.degrees(
                            math.atan(loiter_rad / nearest_distance))
                        tan_dis = math.sqrt(nearest_distance *
                                            nearest_distance -
                                            (loiter_rad * loiter_rad))

                        ral_bearing = mp_util.gps_bearing(
                            nearest_land_wp.x, nearest_land_wp.y,
                            rp.lat * 1.0e-7, rp.lng * 1.0e-7)

                        points.append(
                            mp_util.gps_newpos(nearest_land_wp.x,
                                               nearest_land_wp.y,
                                               ral_bearing + theta, tan_dis))

                    else:  #not tangential approach
                        points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7))

                    points.append((nearest_land_wp.x, nearest_land_wp.y))
                    self.map.add_object(
                        mp_slipmap.SlipPolygon('Rally Land %u' % (i + 1),
                                               points, 'RallyPoints',
                                               (255, 255, 0), 2))

        # check for any events from the map
        self.map.check_events()
Exemple #30
0
def mavflightview(filename):
    print("Loading %s ..." % filename)
    mlog = mavutil.mavlink_connection(filename)
    wp = mavwp.MAVWPLoader()
    if opts.mission is not None:
        wp.load(opts.mission)
    fen = mavwp.MAVFenceLoader()
    if opts.fence is not None:
        fen.load(opts.fence)
    path = [[]]
    types = ['MISSION_ITEM', 'CMD']
    if opts.rawgps:
        types.extend(['GPS', 'GPS_RAW_INT'])
    if opts.rawgps2:
        types.extend(['GPS2_RAW', 'GPS2'])
    if opts.dualgps:
        types.extend(['GPS2_RAW', 'GPS2', 'GPS_RAW_INT', 'GPS'])
    if opts.ekf:
        types.extend(['EKF1', 'GPS'])
    if opts.ahr2:
        types.extend(['AHR2', 'GPS'])
    if len(types) == 2:
        types.extend(['GPS', 'GLOBAL_POSITION_INT'])
    print("Looking for types %s" % str(types))
    while True:
        try:
            m = mlog.recv_match(type=types)
            if m is None:
                break
        except Exception:
            break
        if m.get_type() == 'MISSION_ITEM':
            try:
                while m.seq > wp.count():
                    print("Adding dummy WP %u" % wp.count())
                    wp.set(m, wp.count())
                wp.set(m, m.seq)
            except Exception:
                pass
            continue
        if m.get_type() == 'CMD':
            m = mavutil.mavlink.MAVLink_mission_item_message(
                0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng,
                m.Alt)
            try:
                while m.seq > wp.count():
                    print("Adding dummy WP %u" % wp.count())
                    wp.set(m, wp.count())
                wp.set(m, m.seq)
            except Exception:
                pass
            continue
        if not mlog.check_condition(opts.condition):
            continue
        if opts.mode is not None and mlog.flightmode.lower(
        ) != opts.mode.lower():
            continue
        if m.get_type() in ['GPS', 'GPS2']:
            status = getattr(m, 'Status', None)
            if status is None:
                status = getattr(m, 'FixType', None)
                if status is None:
                    print("Can't find status on GPS message")
                    print(m)
                    break
            if status < 2:
                continue
            # flash log
            lat = m.Lat
            lng = getattr(m, 'Lng', None)
            if lng is None:
                lng = getattr(m, 'Lon', None)
                if lng is None:
                    print("Can't find longitude on GPS message")
                    print(m)
                    break
        elif m.get_type() == 'EKF1':
            pos = mavextra.ekf1_pos(m)
            if pos is None:
                continue
            (lat, lng) = pos
        elif m.get_type() == 'AHR2':
            (lat, lng) = (m.Lat, m.Lng)
        else:
            lat = m.lat * 1.0e-7
            lng = m.lon * 1.0e-7
        instance = 0
        if opts.dualgps and m.get_type() in ['GPS2_RAW', 'GPS2']:
            instance = 1
        if m.get_type() == 'EKF1':
            if opts.dualgps:
                instance = 2
            else:
                instance = 1
        if m.get_type() == 'AHR2':
            if opts.dualgps:
                instance = 2
            else:
                instance = 1
        if abs(lat) > 0.01 or abs(lng) > 0.01:
            if getattr(mlog, 'flightmode', '') in colourmap:
                colour = colourmap[mlog.flightmode]
                (r, g, b) = colour
                (r, g, b) = (r + instance * 50, g + instance * 50,
                             b + instance * 50)
                if r > 255:
                    r = 205
                if g > 255:
                    g = 205
                if b > 255:
                    b = 205
                colour = (r, g, b)
                point = (lat, lng, colour)
            else:
                point = (lat, lng)
            while instance >= len(path):
                path.append([])
            path[instance].append(point)
    if len(path[0]) == 0:
        print("No points to plot")
        return
    bounds = mp_util.polygon_bounds(path[0])
    (lat, lon) = (bounds[0] + bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
                                        lon + bounds[3])
    while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
           >= ground_width - 20 or mp_util.gps_distance(
               lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
        ground_width += 10

    path_objs = []
    for i in range(len(path)):
        if len(path[i]) != 0:
            path_objs.append(
                mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i, filename),
                                       path[i],
                                       layer='FlightPath',
                                       linewidth=2,
                                       colour=(255, 0, 180)))
    plist = wp.polygon_list()
    mission_obj = None
    if len(plist) > 0:
        mission_obj = []
        for i in range(len(plist)):
            mission_obj.append(
                mp_slipmap.SlipPolygon('Mission-%s-%u' % (filename, i),
                                       plist[i],
                                       layer='Mission',
                                       linewidth=2,
                                       colour=(255, 255, 255)))
    else:
        mission_obj = None

    fence = fen.polygon()
    if len(fence) > 1:
        fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename,
                                           fen.polygon(),
                                           layer='Fence',
                                           linewidth=2,
                                           colour=(0, 255, 0))
    else:
        fence_obj = None

    if opts.imagefile:
        create_imagefile(opts.imagefile, (lat, lon), ground_width, path_objs,
                         mission_obj, fence_obj)
    else:
        global multi_map
        if opts.multi and multi_map is not None:
            map = multi_map
        else:
            map = mp_slipmap.MPSlipMap(title=filename,
                                       service=opts.service,
                                       elevation=True,
                                       width=600,
                                       height=600,
                                       ground_width=ground_width,
                                       lat=lat,
                                       lon=lon,
                                       debug=opts.debug)
        if opts.multi:
            multi_map = map
        for path_obj in path_objs:
            map.add_object(path_obj)
        if mission_obj is not None:
            display_waypoints(wp, map)
        if fence_obj is not None:
            map.add_object(fence_obj)

        for flag in opts.flag:
            a = flag.split(',')
            lat = a[0]
            lon = a[1]
            icon = 'flag.png'
            if len(a) > 2:
                icon = a[2] + '.png'
            icon = map.icon(icon)
            map.add_object(
                mp_slipmap.SlipIcon('icon - %s' % str(flag),
                                    (float(lat), float(lon)),
                                    icon,
                                    layer=3,
                                    rotation=0,
                                    follow=False))
Exemple #31
0
    def cmd_wp_movemulti(self, args):
        '''handle wp move of multiple waypoints'''
        if len(args) < 4:
            print(
                "usage: wp movemulti WPNUM WPSTART WPEND <rotation> <newLocation>"
            )
            return
        idx = int(args[0])
        if idx < 1 or idx > self.wploader.count():
            print("Invalid wp number %u" % idx)
            return
        wpstart = int(args[1])
        if wpstart < 1 or wpstart > self.wploader.count():
            print("Invalid wp number %u" % wpstart)
            return
        wpend = int(args[2])
        if wpend < 1 or wpend > self.wploader.count():
            print("Invalid wp number %u" % wpend)
            return
        if idx < wpstart or idx > wpend:
            print("WPNUM must be between WPSTART and WPEND")
            return

        # optional rotation about center point
        if len(args) > 3:
            rotation = float(args[3])
        else:
            rotation = 0

        # optional --> Override mouse click and use a waypoint for the move location instead
        if len(args) > 4:
            wpmove = int(args[4])
            if wpmove < 0 or wpmove > self.wploader.count():
                print("Invalid wp number %u" % wpmove)
                return
            wp = self.wploader.wp(wpmove)
            latlon = (wp.x, wp.y)
        else:
            try:
                latlon = self.module('map').click_position
            except Exception:
                print("No map available")
                return
            if latlon is None:
                print("No map click position available")
                return

        wp = self.wploader.wp(idx)
        if not self.wploader.is_location_command(wp.command):
            print("WP must be a location command")
            return

        (lat, lon) = latlon
        distance = mp_util.gps_distance(wp.x, wp.y, lat, lon)
        bearing = mp_util.gps_bearing(wp.x, wp.y, lat, lon)

        for wpnum in range(wpstart, wpend + 1):
            wp = self.wploader.wp(wpnum)
            if not self.wploader.is_location_command(wp.command):
                continue
            (newlat, newlon) = mp_util.gps_newpos(wp.x, wp.y, bearing,
                                                  distance)
            if wpnum != idx and rotation != 0:
                # add in rotation
                d2 = mp_util.gps_distance(lat, lon, newlat, newlon)
                b2 = mp_util.gps_bearing(lat, lon, newlat, newlon)
                (newlat, newlon) = mp_util.gps_newpos(lat, lon, b2 + rotation,
                                                      d2)

            if getattr(
                    self.console, 'ElevationMap', None
            ) is not None and wp.frame != mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT:
                alt1 = self.console.ElevationMap.GetElevation(newlat, newlon)
                alt2 = self.console.ElevationMap.GetElevation(wp.x, wp.y)
                if alt1 is not None and alt2 is not None:
                    wp.z += alt1 - alt2
            wp.x = newlat
            wp.y = newlon
            wp.target_system = self.target_system
            wp.target_component = self.target_component
            self.wploader.set(wp, wpnum)

        self.loading_waypoints = True
        self.loading_waypoint_lasttime = time.time()
        self.master.mav.mission_write_partial_list_send(
            self.target_system, self.target_component, wpstart, wpend + 1)
        print("Moved WPs %u:%u to %f, %f rotation=%.1f" %
              (wpstart, wpend, lat, lon, rotation))
Exemple #32
0
def mavflightview(filename):
    print("Loading %s ..." % filename)
    mlog = mavutil.mavlink_connection(filename)
    wp = mavwp.MAVWPLoader()
    if opts.mission is not None:
        wp.load(opts.mission)
    path = []
    while True:
        m = mlog.recv_match(type=['MISSION_ITEM', 'GLOBAL_POSITION_INT'])
        if m is None:
            break
        if m.get_type() == 'GLOBAL_POSITION_INT' and mlog.check_condition(
                opts.condition):
            if opts.mode is not None and mlog.flightmode.lower(
            ) != opts.mode.lower():
                continue
            lat = m.lat * 1.0e-7
            lng = m.lon * 1.0e-7
            if lat != 0 or lng != 0:
                if mlog.flightmode in colourmap:
                    point = (lat, lng, colourmap[mlog.flightmode])
                else:
                    point = (lat, lng)
                path.append(point)
        if m.get_type() == 'MISSION_ITEM':
            wp.set(m, m.seq)
    if len(path) == 0:
        print("No points to plot")
        return
    bounds = mp_util.polygon_bounds(path)
    (lat, lon) = (bounds[0] + bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
                                        lon + bounds[3])
    while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
           >= ground_width - 20 or mp_util.gps_distance(
               lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
        ground_width += 10

    path_obj = mp_slipmap.SlipPolygon('FlightPath',
                                      path,
                                      layer='FlightPath',
                                      linewidth=2,
                                      colour=(255, 0, 180))
    mission = wp.polygon()
    if len(mission) > 1:
        mission_obj = mp_slipmap.SlipPolygon('Mission',
                                             wp.polygon(),
                                             layer='Mission',
                                             linewidth=2,
                                             colour=(255, 255, 255))
    else:
        mission_obj = None

    if opts.imagefile:
        create_imagefile(opts.imagefile, (lat, lon), ground_width, path_obj,
                         mission_obj)
    else:
        map = mp_slipmap.MPSlipMap(title=filename,
                                   service=opts.service,
                                   elevation=True,
                                   width=600,
                                   height=600,
                                   ground_width=ground_width,
                                   lat=lat,
                                   lon=lon)
        map.add_object(path_obj)
        if mission_obj is not None:
            map.add_object(mission_obj)

        for flag in opts.flag:
            a = flag.split(',')
            lat = a[0]
            lon = a[1]
            icon = 'flag.png'
            if len(a) > 2:
                icon = a[2] + '.png'
            icon = map.icon(icon)
            map.add_object(
                mp_slipmap.SlipIcon('icon - %s' % str(flag),
                                    (float(lat), float(lon)),
                                    icon,
                                    layer=3,
                                    rotation=0,
                                    follow=False))
Exemple #33
0
    def mavlink_packet(self, m):
        '''handle an incoming mavlink packet'''
        if m.get_type() == "HEARTBEAT":
            from pymavlink import mavutil
            if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
                self.vehicle_type_name = 'plane'
            elif m.type in [
                    mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
                    mavutil.mavlink.MAV_TYPE_SURFACE_BOAT,
                    mavutil.mavlink.MAV_TYPE_SUBMARINE
            ]:
                self.vehicle_type_name = 'rover'
            elif m.type in [
                    mavutil.mavlink.MAV_TYPE_QUADROTOR,
                    mavutil.mavlink.MAV_TYPE_COAXIAL,
                    mavutil.mavlink.MAV_TYPE_HEXAROTOR,
                    mavutil.mavlink.MAV_TYPE_OCTOROTOR,
                    mavutil.mavlink.MAV_TYPE_TRICOPTER,
                    mavutil.mavlink.MAV_TYPE_HELICOPTER
            ]:
                self.vehicle_type_name = 'copter'
            elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
                self.vehicle_type_name = 'antenna'

        # this is the beginnings of allowing support for multiple vehicles
        # in the air at the same time
        vehicle = 'Vehicle%u' % m.get_srcSystem()

        if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos:
            self.create_vehicle_icon('Sim' + vehicle, 'green')
            self.mpstate.map.set_position('Sim' + vehicle,
                                          (m.lat * 1.0e-7, m.lng * 1.0e-7),
                                          rotation=math.degrees(m.yaw))

        if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos:
            self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
            self.mpstate.map.set_position('AHRS2' + vehicle,
                                          (m.lat * 1.0e-7, m.lng * 1.0e-7),
                                          rotation=math.degrees(m.yaw))

        if m.get_type() == "GPS_RAW_INT" and self.map_settings.showgpspos:
            (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
            if lat != 0 or lon != 0:
                self.create_vehicle_icon('GPS' + vehicle, 'blue')
                self.mpstate.map.set_position('GPS' + vehicle, (lat, lon),
                                              rotation=m.cog * 0.01)

        if m.get_type() == "GPS2_RAW" and self.map_settings.showgps2pos:
            (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
            if lat != 0 or lon != 0:
                self.create_vehicle_icon('GPS2' + vehicle, 'green')
                self.mpstate.map.set_position('GPS2' + vehicle, (lat, lon),
                                              rotation=m.cog * 0.01)

        if m.get_type() == 'GLOBAL_POSITION_INT':
            (self.lat, self.lon, self.heading) = (m.lat * 1.0e-7,
                                                  m.lon * 1.0e-7, m.hdg * 0.01)
            if self.lat != 0 or self.lon != 0:
                self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
                self.mpstate.map.set_position('Pos' + vehicle,
                                              (self.lat, self.lon),
                                              rotation=self.heading)

        if m.get_type() == "NAV_CONTROLLER_OUTPUT":
            if self.master.flightmode in ["AUTO", "GUIDED", "LOITER", "RTL"]:
                trajectory = [(self.lat, self.lon),
                              mp_util.gps_newpos(self.lat, self.lon,
                                                 m.target_bearing, m.wp_dist)]
                self.mpstate.map.add_object(
                    mp_slipmap.SlipPolygon('trajectory',
                                           trajectory,
                                           layer='Trajectory',
                                           linewidth=2,
                                           colour=(255, 0, 180)))
            else:
                self.mpstate.map.add_object(
                    mp_slipmap.SlipClearLayer('Trajectory'))

        # if the waypoints have changed, redisplay
        if self.wp_change_time != self.module('wp').wploader.last_change:
            self.wp_change_time = self.module('wp').wploader.last_change
            self.display_waypoints()

        # if the fence has changed, redisplay
        if self.fence_change_time != self.module(
                'fence').fenceloader.last_change:
            self.display_fence()

        # if the rallypoints have changed, redisplay
        if self.rally_change_time != self.module(
                'rally').rallyloader.last_change:
            self.rally_change_time = self.module(
                'rally').rallyloader.last_change
            icon = self.mpstate.map.icon('rallypoint.png')
            self.mpstate.map.add_object(
                mp_slipmap.SlipClearLayer('RallyPoints'))
            for i in range(self.module('rally').rallyloader.rally_count()):
                rp = self.module('rally').rallyloader.rally_point(i)
                popup = MPMenuSubMenu(
                    'Popup',
                    items=[
                        MPMenuItem('Rally Remove',
                                   returnkey='popupRallyRemove'),
                        MPMenuItem('Rally Move', returnkey='popupRallyMove')
                    ])
                self.mpstate.map.add_object(
                    mp_slipmap.SlipIcon('Rally %u' % (i + 1),
                                        (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
                                        icon,
                                        layer='RallyPoints',
                                        rotation=0,
                                        follow=False,
                                        popup_menu=popup))

        # check for any events from the map
        self.mpstate.map.check_events()
Exemple #34
0
def mavflightview_show(path, wp, fen, options, title=None):
    if not title:
        title='MAVFlightView'

    bounds = mp_util.polygon_bounds(path[0])
    (lat, lon) = (bounds[0]+bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
    while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
           mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
        ground_width += 10

    path_objs = []
    for i in range(len(path)):
        if len(path[i]) != 0:
            path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,title), path[i], layer='FlightPath',
                                                    linewidth=2, colour=(255,0,180)))
    plist = wp.polygon_list()
    mission_obj = None
    if len(plist) > 0:
        mission_obj = []
        for i in range(len(plist)):
            mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (title,i), plist[i], layer='Mission',
                                                      linewidth=2, colour=(255,255,255)))
    else:
        mission_obj = None

    fence = fen.polygon()
    if len(fence) > 1:
        fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence',
                                           linewidth=2, colour=(0,255,0))
    else:
        fence_obj = None

    if options.imagefile:
        create_imagefile(options, options.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
    else:
        global multi_map
        if options.multi and multi_map is not None:
            map = multi_map
        else:
            map = mp_slipmap.MPSlipMap(title=title,
                                       service=options.service,
                                       elevation=True,
                                       width=600,
                                       height=600,
                                       ground_width=ground_width,
                                       lat=lat, lon=lon,
                                       debug=options.debug)
        if options.multi:
            multi_map = map
        for path_obj in path_objs:
            map.add_object(path_obj)
        if mission_obj is not None:
            display_waypoints(wp, map)
        if fence_obj is not None:
            map.add_object(fence_obj)

        for flag in options.flag:
            a = flag.split(',')
            lat = a[0]
            lon = a[1]
            icon = 'flag.png'
            if len(a) > 2:
                icon = a[2] + '.png'
            icon = map.icon(icon)
            map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))

    source = getattr(options, "colour_source", "flightmode")
    if source != "flightmode":
        print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
Exemple #35
0
def mavflightview_mav(mlog, options=None, title=None):
    '''create a map for a log file'''
    if not title:
        title = 'MAVFlightView'
    wp = mavwp.MAVWPLoader()
    if options.mission is not None:
        wp.load(options.mission)
    fen = mavwp.MAVFenceLoader()
    if options.fence is not None:
        fen.load(options.fence)
    path = [[]]
    instances = {}
    ekf_counter = 0
    nkf_counter = 0
    types = ['MISSION_ITEM', 'CMD']
    if options.types is not None:
        types.extend(options.types.split(','))
    else:
        types.extend(['GPS', 'GLOBAL_POSITION_INT'])
        if options.rawgps or options.dualgps:
            types.extend(['GPS', 'GPS_RAW_INT'])
        if options.rawgps2 or options.dualgps:
            types.extend(['GPS2_RAW', 'GPS2'])
        if options.ekf:
            types.extend(['EKF1', 'GPS'])
        if options.nkf:
            types.extend(['NKF1', 'GPS'])
        if options.ahr2:
            types.extend(['AHR2', 'AHRS2', 'GPS'])
    print("Looking for types %s" % str(types))

    last_timestamps = {}

    while True:
        try:
            m = mlog.recv_match(type=types)
            if m is None:
                break
        except Exception:
            break

        type = m.get_type()

        if type == 'MISSION_ITEM':
            try:
                while m.seq > wp.count():
                    print("Adding dummy WP %u" % wp.count())
                    wp.set(m, wp.count())
                wp.set(m, m.seq)
            except Exception:
                pass
            continue
        if type == 'CMD':
            m = mavutil.mavlink.MAVLink_mission_item_message(
                0, 0, m.CNum, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3, m.Prm4, m.Lat, m.Lng,
                m.Alt)
            try:
                while m.seq > wp.count():
                    print("Adding dummy WP %u" % wp.count())
                    wp.set(m, wp.count())
                wp.set(m, m.seq)
            except Exception:
                pass
            continue
        if not mlog.check_condition(options.condition):
            continue
        if options.mode is not None and mlog.flightmode.lower(
        ) != options.mode.lower():
            continue
        if type in ['GPS', 'GPS2']:
            status = getattr(m, 'Status', None)
            if status is None:
                status = getattr(m, 'FixType', None)
                if status is None:
                    print("Can't find status on GPS message")
                    print(m)
                    break
            if status < 2:
                continue
            # flash log
            lat = m.Lat
            lng = getattr(m, 'Lng', None)
            if lng is None:
                lng = getattr(m, 'Lon', None)
                if lng is None:
                    print("Can't find longitude on GPS message")
                    print(m)
                    break
        elif type in ['EKF1', 'ANU1']:
            pos = mavextra.ekf1_pos(m)
            if pos is None:
                continue
            ekf_counter += 1
            if ekf_counter % options.ekf_sample != 0:
                continue
            (lat, lng) = pos
        elif type in ['NKF1']:
            pos = mavextra.ekf1_pos(m)
            if pos is None:
                continue
            nkf_counter += 1
            if nkf_counter % options.nkf_sample != 0:
                continue
            (lat, lng) = pos
        elif type in ['ANU5']:
            (lat, lng) = (m.Alat * 1.0e-7, m.Alng * 1.0e-7)
        elif type in ['AHR2', 'POS', 'CHEK']:
            (lat, lng) = (m.Lat, m.Lng)
        elif type == 'AHRS2':
            (lat, lng) = (m.lat * 1.0e-7, m.lng * 1.0e-7)
        else:
            lat = m.lat * 1.0e-7
            lng = m.lon * 1.0e-7

        # automatically add new types to instances
        if type not in instances:
            instances[type] = len(instances)
            while len(instances) >= len(path):
                path.append([])
        instance = instances[type]

        if abs(lat) > 0.01 or abs(lng) > 0.01:
            colour = colour_for_point(mlog, (lat, lng), instance, options)
            point = (lat, lng, colour)

            if options.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[
                    type] > 1.0 / options.rate:
                last_timestamps[type] = m._timestamp
                path[instance].append(point)
    if len(path[0]) == 0:
        print("No points to plot")
        return
    bounds = mp_util.polygon_bounds(path[0])
    (lat, lon) = (bounds[0] + bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2],
                                        lon + bounds[3])
    while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1])
           >= ground_width - 20 or mp_util.gps_distance(
               lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20):
        ground_width += 10

    path_objs = []
    for i in range(len(path)):
        if len(path[i]) != 0:
            path_objs.append(
                mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i, title),
                                       path[i],
                                       layer='FlightPath',
                                       linewidth=2,
                                       colour=(255, 0, 180)))
    plist = wp.polygon_list()
    mission_obj = None
    if len(plist) > 0:
        mission_obj = []
        for i in range(len(plist)):
            mission_obj.append(
                mp_slipmap.SlipPolygon('Mission-%s-%u' % (title, i),
                                       plist[i],
                                       layer='Mission',
                                       linewidth=2,
                                       colour=(255, 255, 255)))
    else:
        mission_obj = None

    fence = fen.polygon()
    if len(fence) > 1:
        fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title,
                                           fen.polygon(),
                                           layer='Fence',
                                           linewidth=2,
                                           colour=(0, 255, 0))
    else:
        fence_obj = None

    if options.imagefile:
        create_imagefile(options, options.imagefile, (lat, lon), ground_width,
                         path_objs, mission_obj, fence_obj)
    else:
        global multi_map
        if options.multi and multi_map is not None:
            map = multi_map
        else:
            map = mp_slipmap.MPSlipMap(title=title,
                                       service=options.service,
                                       elevation=True,
                                       width=600,
                                       height=600,
                                       ground_width=ground_width,
                                       lat=lat,
                                       lon=lon,
                                       debug=options.debug)
        if options.multi:
            multi_map = map
        for path_obj in path_objs:
            map.add_object(path_obj)
        if mission_obj is not None:
            display_waypoints(wp, map)
        if fence_obj is not None:
            map.add_object(fence_obj)

        for flag in options.flag:
            a = flag.split(',')
            lat = a[0]
            lon = a[1]
            icon = 'flag.png'
            if len(a) > 2:
                icon = a[2] + '.png'
            icon = map.icon(icon)
            map.add_object(
                mp_slipmap.SlipIcon('icon - %s' % str(flag),
                                    (float(lat), float(lon)),
                                    icon,
                                    layer=3,
                                    rotation=0,
                                    follow=False))

    source = getattr(options, "colour_source", "flightmode")
    if source != "flightmode":
        print("colour-source: min=%f max=%f" %
              (colour_source_min, colour_source_max))
Exemple #36
0
def mavflightview(filename):
    print("Loading %s ..." % filename)
    mlog = mavutil.mavlink_connection(filename)
    wp = mavwp.MAVWPLoader()
    if opts.mission is not None:
        wp.load(opts.mission)
    path = []
    while True:
        m = mlog.recv_match(type=['MISSION_ITEM', 'GLOBAL_POSITION_INT', 'GPS_RAW_INT', 'GPS'])
        if m is None:
            break
        if m.get_type() == 'MISSION_ITEM':
            wp.set(m, m.seq)            
            continue
        if m.get_type() == 'GLOBAL_POSITION_INT' and opts.rawgps:
            continue
        if m.get_type() == 'GPS_RAW_INT' and not opts.rawgps:
            continue
        if not mlog.check_condition(opts.condition):
            continue
        if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
            continue
        if m.get_type() == 'GPS':
            if m.Status < 2:
                continue
            # flash log
            lat = m.Lat
            lng = m.Lng
        else:
            lat = m.lat * 1.0e-7
            lng = m.lon * 1.0e-7
        if lat != 0 or lng != 0:
            if getattr(mlog, 'flightmode','') in colourmap:
                point = (lat, lng, colourmap[mlog.flightmode])
            else:
                point = (lat, lng)
            path.append(point)
    if len(path) == 0:
        print("No points to plot")
        return
    bounds = mp_util.polygon_bounds(path)
    (lat, lon) = (bounds[0]+bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
    while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
           mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
        ground_width += 10

    path_obj = mp_slipmap.SlipPolygon('FlightPath', path, layer='FlightPath',
                                      linewidth=2, colour=(255,0,180))
    mission = wp.polygon()
    if len(mission) > 1:
        mission_obj = mp_slipmap.SlipPolygon('Mission', wp.polygon(), layer='Mission',
                                             linewidth=2, colour=(255,255,255))
    else:
        mission_obj = None

    if opts.imagefile:
        create_imagefile(opts.imagefile, (lat,lon), ground_width, path_obj, mission_obj)
    else:
        map = mp_slipmap.MPSlipMap(title=filename,
                                   service=opts.service,
                                   elevation=True,
                                   width=600,
                                   height=600,
                                   ground_width=ground_width,
                                   lat=lat, lon=lon,
                                   debug=opts.debug)
        map.add_object(path_obj)
        if mission_obj is not None:
            map.add_object(mission_obj)

        for flag in opts.flag:
            a = flag.split(',')
            lat = a[0]
            lon = a[1]
            icon = 'flag.png'
            if len(a) > 2:
                icon = a[2] + '.png'
            icon = map.icon(icon)
            map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
Exemple #37
0
    def cmd_wp_movemulti(self, args):
        '''handle wp move of multiple waypoints'''
        if len(args) < 3:
            print("usage: wp movemulti WPNUM WPSTART WPEND <rotation>")
            return
        idx = int(args[0])
        if idx < 1 or idx > self.wploader.count():
            print("Invalid wp number %u" % idx)
            return
        wpstart = int(args[1])
        if wpstart < 1 or wpstart > self.wploader.count():
            print("Invalid wp number %u" % wpstart)
            return
        wpend = int(args[2])
        if wpend < 1 or wpend > self.wploader.count():
            print("Invalid wp number %u" % wpend)
            return
        if idx < wpstart or idx > wpend:
            print("WPNUM must be between WPSTART and WPEND")
            return

        # optional rotation about center point
        if len(args) > 3:
            rotation = float(args[3])
        else:
            rotation = 0

        try:
            latlon = self.module('map').click_position
        except Exception:
            print("No map available")
            return
        if latlon is None:
            print("No map click position available")
            return
        wp = self.wploader.wp(idx)
        if not self.wploader.is_location_command(wp.command):
            print("WP must be a location command")
            return

        (lat, lon) = latlon
        distance = mp_util.gps_distance(wp.x, wp.y, lat, lon)
        bearing  = mp_util.gps_bearing(wp.x, wp.y, lat, lon)

        for wpnum in range(wpstart, wpend+1):
            wp = self.wploader.wp(wpnum)
            if not self.wploader.is_location_command(wp.command):
                continue
            (newlat, newlon) = mp_util.gps_newpos(wp.x, wp.y, bearing, distance)
            if wpnum != idx and rotation != 0:
                # add in rotation
                d2 = mp_util.gps_distance(lat, lon, newlat, newlon)
                b2 = mp_util.gps_bearing(lat, lon, newlat, newlon)
                (newlat, newlon) = mp_util.gps_newpos(lat, lon, b2+rotation, d2)

            if getattr(self.console, 'ElevationMap', None) is not None and wp.frame != mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT:
                alt1 = self.console.ElevationMap.GetElevation(newlat, newlon)
                alt2 = self.console.ElevationMap.GetElevation(wp.x, wp.y)
                if alt1 is not None and alt2 is not None:
                    wp.z += alt1 - alt2
            wp.x = newlat
            wp.y = newlon
            wp.target_system    = self.target_system
            wp.target_component = self.target_component
            self.wploader.set(wp, wpnum)

        self.loading_waypoints = True
        self.loading_waypoint_lasttime = time.time()
        self.master.mav.mission_write_partial_list_send(self.target_system,
                                                        self.target_component,
                                                        wpstart, wpend+1)
        print("Moved WPs %u:%u to %f, %f rotation=%.1f" % (wpstart, wpend, lat, lon, rotation))
Exemple #38
0
    def mavlink_packet(self, m):
        """handle an incoming mavlink packet"""
        if m.get_type() == "HEARTBEAT":
            from pymavlink import mavutil

            if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
                self.vehicle_type_name = "plane"
            elif m.type in [
                mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
                mavutil.mavlink.MAV_TYPE_SURFACE_BOAT,
                mavutil.mavlink.MAV_TYPE_SUBMARINE,
            ]:
                self.vehicle_type_name = "rover"
            elif m.type in [
                mavutil.mavlink.MAV_TYPE_QUADROTOR,
                mavutil.mavlink.MAV_TYPE_COAXIAL,
                mavutil.mavlink.MAV_TYPE_HEXAROTOR,
                mavutil.mavlink.MAV_TYPE_OCTOROTOR,
                mavutil.mavlink.MAV_TYPE_TRICOPTER,
                mavutil.mavlink.MAV_TYPE_HELICOPTER,
            ]:
                self.vehicle_type_name = "copter"
            elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
                self.vehicle_type_name = "antenna"

        # this is the beginnings of allowing support for multiple vehicles
        # in the air at the same time
        vehicle = "Vehicle%u" % m.get_srcSystem()

        if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos:
            self.create_vehicle_icon("Sim" + vehicle, "green")
            self.mpstate.map.set_position(
                "Sim" + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)
            )

        if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos:
            self.create_vehicle_icon("AHRS2" + vehicle, "blue")
            self.mpstate.map.set_position(
                "AHRS2" + vehicle, (m.lat * 1.0e-7, m.lng * 1.0e-7), rotation=math.degrees(m.yaw)
            )

        if m.get_type() == "GPS_RAW_INT" and self.map_settings.showgpspos:
            (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
            if lat != 0 or lon != 0:
                self.create_vehicle_icon("GPS" + vehicle, "blue")
                self.mpstate.map.set_position("GPS" + vehicle, (lat, lon), rotation=m.cog * 0.01)

        if m.get_type() == "GPS2_RAW" and self.map_settings.showgps2pos:
            (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
            if lat != 0 or lon != 0:
                self.create_vehicle_icon("GPS2" + vehicle, "green")
                self.mpstate.map.set_position("GPS2" + vehicle, (lat, lon), rotation=m.cog * 0.01)

        if m.get_type() == "GLOBAL_POSITION_INT":
            (self.lat, self.lon, self.heading) = (m.lat * 1.0e-7, m.lon * 1.0e-7, m.hdg * 0.01)
            if self.lat != 0 or self.lon != 0:
                self.create_vehicle_icon("Pos" + vehicle, "red", follow=True)
                self.mpstate.map.set_position("Pos" + vehicle, (self.lat, self.lon), rotation=self.heading)

        if m.get_type() == "NAV_CONTROLLER_OUTPUT":
            if self.master.flightmode in ["AUTO", "GUIDED", "LOITER", "RTL"]:
                trajectory = [(self.lat, self.lon), mp_util.gps_newpos(self.lat, self.lon, m.target_bearing, m.wp_dist)]
                self.mpstate.map.add_object(
                    mp_slipmap.SlipPolygon(
                        "trajectory", trajectory, layer="Trajectory", linewidth=2, colour=(255, 0, 180)
                    )
                )
            else:
                self.mpstate.map.add_object(mp_slipmap.SlipClearLayer("Trajectory"))

        # if the waypoints have changed, redisplay
        last_wp_change = self.module("wp").wploader.last_change
        if self.wp_change_time != last_wp_change and abs(time.time() - last_wp_change) > 1:
            self.wp_change_time = last_wp_change
            self.display_waypoints()

            # this may have affected the landing lines from the rally points:
            self.rally_change_time = time.time()

        # if the fence has changed, redisplay
        if self.fence_change_time != self.module("fence").fenceloader.last_change:
            self.display_fence()

        # if the rallypoints have changed, redisplay
        if self.rally_change_time != self.module("rally").rallyloader.last_change:
            self.rally_change_time = self.module("rally").rallyloader.last_change
            icon = self.mpstate.map.icon("rallypoint.png")
            self.mpstate.map.add_object(mp_slipmap.SlipClearLayer("RallyPoints"))
            for i in range(self.module("rally").rallyloader.rally_count()):
                rp = self.module("rally").rallyloader.rally_point(i)
                popup = MPMenuSubMenu(
                    "Popup",
                    items=[
                        MPMenuItem("Rally Remove", returnkey="popupRallyRemove"),
                        MPMenuItem("Rally Move", returnkey="popupRallyMove"),
                    ],
                )
                self.mpstate.map.add_object(
                    mp_slipmap.SlipIcon(
                        "Rally %u" % (i + 1),
                        (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
                        icon,
                        layer="RallyPoints",
                        rotation=0,
                        follow=False,
                        popup_menu=popup,
                    )
                )

                loiter_rad = self.get_mav_param("WP_LOITER_RAD")

                if self.map_settings.rallycircle:
                    self.mpstate.map.add_object(
                        mp_slipmap.SlipCircle(
                            "Rally Circ %u" % (i + 1),
                            "RallyPoints",
                            (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
                            abs(loiter_rad),
                            (255, 255, 0),
                            2,
                        )
                    )

                # draw a line between rally point and nearest landing point
                nearest_land_wp = None
                nearest_distance = 10000000.0
                for j in range(self.module("wp").wploader.count()):
                    w = self.module("wp").wploader.wp(j)
                    if w.command == 21:  # if landing waypoint
                        # get distance between rally point and this waypoint
                        dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7, rp.lng * 1.0e-7)
                        if dis < nearest_distance:
                            nearest_land_wp = w
                            nearest_distance = dis

                if nearest_land_wp != None:
                    points = []
                    # tangential approach?
                    if self.get_mav_param("LAND_BREAK_PATH") == 0:
                        theta = math.degrees(math.atan(loiter_rad / nearest_distance))
                        tan_dis = math.sqrt(nearest_distance * nearest_distance - (loiter_rad * loiter_rad))

                        ral_bearing = mp_util.gps_bearing(
                            nearest_land_wp.x, nearest_land_wp.y, rp.lat * 1.0e-7, rp.lng * 1.0e-7
                        )

                        points.append(
                            mp_util.gps_newpos(nearest_land_wp.x, nearest_land_wp.y, ral_bearing + theta, tan_dis)
                        )

                    else:  # not tangential approach
                        points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7))

                    points.append((nearest_land_wp.x, nearest_land_wp.y))
                    self.mpstate.map.add_object(
                        mp_slipmap.SlipPolygon("Rally Land %u" % (i + 1), points, "RallyPoints", (255, 255, 0), 2)
                    )

        # check for any events from the map
        self.mpstate.map.check_events()
Exemple #39
0
def mavflightview(filename):
    print("Loading %s ..." % filename)
    mlog = mavutil.mavlink_connection(filename)
    wp = mavwp.MAVWPLoader()
    if opts.mission is not None:
        wp.load(opts.mission)
    fen = mavwp.MAVFenceLoader()
    if opts.fence is not None:
        fen.load(opts.fence)
    path = [[]]
    instances = {}
    types = ['MISSION_ITEM','CMD']
    if opts.types is not None:
        types.extend(opts.types.split(','))
    else:
        types.extend(['GPS','GLOBAL_POSITION_INT'])
        if opts.rawgps or opts.dualgps:
            types.extend(['GPS', 'GPS_RAW_INT'])
        if opts.rawgps2 or opts.dualgps:
            types.extend(['GPS2_RAW','GPS2'])
        if opts.ekf:
            types.extend(['EKF1', 'GPS'])
        if opts.ahr2:
            types.extend(['AHR2', 'AHRS2', 'GPS'])
    print("Looking for types %s" % str(types))
    while True:
        try:
            m = mlog.recv_match(type=types)
            if m is None:
                break
        except Exception:
            break
        if m.get_type() == 'MISSION_ITEM':
            try:
                while m.seq > wp.count():
                    print("Adding dummy WP %u" % wp.count())
                    wp.set(m, wp.count())
                wp.set(m, m.seq)
            except Exception:
                pass
            continue
        if m.get_type() == 'CMD':
            m = mavutil.mavlink.MAVLink_mission_item_message(0,
                                                             0,
                                                             m.CNum,
                                                             mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                                                             m.CId,
                                                             0, 1,
                                                             m.Prm1, m.Prm2, m.Prm3, m.Prm4,
                                                             m.Lat, m.Lng, m.Alt)
            try:
                while m.seq > wp.count():
                    print("Adding dummy WP %u" % wp.count())
                    wp.set(m, wp.count())
                wp.set(m, m.seq)
            except Exception:
                pass
            continue
        if not mlog.check_condition(opts.condition):
            continue
        if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
            continue
        if m.get_type() in ['GPS','GPS2']:
            status = getattr(m, 'Status', None)
            if status is None:
                status = getattr(m, 'FixType', None)
                if status is None:
                    print("Can't find status on GPS message")
                    print(m)
                    break
            if status < 2:
                continue
            # flash log
            lat = m.Lat
            lng = getattr(m, 'Lng', None)
            if lng is None:
                lng = getattr(m, 'Lon', None)
                if lng is None:
                    print("Can't find longitude on GPS message")
                    print(m)
                    break                    
        elif m.get_type() == 'EKF1':
            pos = mavextra.ekf1_pos(m)
            if pos is None:
                continue
            (lat, lng) = pos            
        elif m.get_type() == 'AHR2':
            (lat, lng) = (m.Lat, m.Lng)
        elif m.get_type() == 'AHRS2':
            (lat, lng) = (m.lat*1.0e-7, m.lng*1.0e-7)
        else:
            lat = m.lat * 1.0e-7
            lng = m.lon * 1.0e-7

        # automatically add new types to instances
        if m.get_type() not in instances:
            instances[m.get_type()] = len(instances)
            while len(instances) >= len(path):
                path.append([])
        instance = instances[m.get_type()]

        if abs(lat)>0.01 or abs(lng)>0.01:
            if getattr(mlog, 'flightmode','') in colourmap:
                colour = colourmap[mlog.flightmode]
                (r,g,b) = colour
                (r,g,b) = (r+instance*50,g+instance*50,b+instance*50)
                if r > 255:
                    r = 205
                if g > 255:
                    g = 205
                if b > 255:
                    b = 205
                colour = (r,g,b)
                point = (lat, lng, colour)
            else:
                point = (lat, lng)
            path[instance].append(point)
    if len(path[0]) == 0:
        print("No points to plot")
        return
    bounds = mp_util.polygon_bounds(path[0])
    (lat, lon) = (bounds[0]+bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
    while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
           mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
        ground_width += 10

    path_objs = []
    for i in range(len(path)):
        if len(path[i]) != 0:
            path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,filename), path[i], layer='FlightPath',
                                                    linewidth=2, colour=(255,0,180)))
    plist = wp.polygon_list()
    mission_obj = None
    if len(plist) > 0:
        mission_obj = []
        for i in range(len(plist)):
            mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (filename,i), plist[i], layer='Mission',
                                                      linewidth=2, colour=(255,255,255)))
    else:
        mission_obj = None

    fence = fen.polygon()
    if len(fence) > 1:
        fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % filename, fen.polygon(), layer='Fence',
                                           linewidth=2, colour=(0,255,0))
    else:
        fence_obj = None

    if opts.imagefile:
        create_imagefile(opts.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj)
    else:
        global multi_map
        if opts.multi and multi_map is not None:
            map = multi_map
        else:
            map = mp_slipmap.MPSlipMap(title=filename,
                                       service=opts.service,
                                       elevation=True,
                                       width=600,
                                       height=600,
                                       ground_width=ground_width,
                                       lat=lat, lon=lon,
                                       debug=opts.debug)
        if opts.multi:
            multi_map = map
        for path_obj in path_objs:
            map.add_object(path_obj)
        if mission_obj is not None:
            display_waypoints(wp, map)
        if fence_obj is not None:
            map.add_object(fence_obj)

        for flag in opts.flag:
            a = flag.split(',')
            lat = a[0]
            lon = a[1]
            icon = 'flag.png'
            if len(a) > 2:
                icon = a[2] + '.png'
            icon = map.icon(icon)
            map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))
Exemple #40
0
            SlipThumbnail('thumb', (opts.lat, opts.lon),
                          layer=1,
                          img=thumb,
                          border_width=2,
                          border_colour=(255, 0, 0)))

    if opts.icon:
        icon = cv2.imread(opts.icon)
        sm.add_object(
            SlipIcon('icon', (opts.lat, opts.lon),
                     icon,
                     layer=3,
                     rotation=90,
                     follow=True))
        sm.set_position('icon',
                        mp_util.gps_newpos(opts.lat, opts.lon, 180, 100),
                        rotation=45)
        sm.add_object(SlipInfoImage('detail', icon))
        sm.add_object(SlipInfoText('detail text', 'test text'))

    for flag in opts.flag:
        (lat, lon) = flag.split(',')
        icon = sm.icon('flag.png')
        sm.add_object(
            SlipIcon('icon - %s' % str(flag), (float(lat), float(lon)),
                     icon,
                     layer=3,
                     rotation=0,
                     follow=False))

    while sm.is_alive():
 def move(self, bearing, distance):
     '''move position by bearing and distance'''
     lat = self.pkt['I105']['Lat']['val']
     lon = self.pkt['I105']['Lon']['val']
     (lat, lon) = mp_util.gps_newpos(lat, lon, bearing, distance)
     self.setpos(lat, lon)
Exemple #42
0
 def mavlink_packet(self, m):
     '''handle an incoming mavlink packet'''
     if m.get_type() == "HEARTBEAT":
         from pymavlink import mavutil
         if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
             self.vehicle_type_name = 'plane'
         elif m.type in [mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
                         mavutil.mavlink.MAV_TYPE_SURFACE_BOAT,
                         mavutil.mavlink.MAV_TYPE_SUBMARINE]:
             self.vehicle_type_name = 'rover'
         elif m.type in [mavutil.mavlink.MAV_TYPE_QUADROTOR,
                         mavutil.mavlink.MAV_TYPE_COAXIAL,
                         mavutil.mavlink.MAV_TYPE_HEXAROTOR,
                         mavutil.mavlink.MAV_TYPE_OCTOROTOR,
                         mavutil.mavlink.MAV_TYPE_TRICOPTER,
                         mavutil.mavlink.MAV_TYPE_HELICOPTER]:
             self.vehicle_type_name = 'copter'
         elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
             self.vehicle_type_name = 'antenna'     
 
     # this is the beginnings of allowing support for multiple vehicles
     # in the air at the same time
     vehicle = 'Vehicle%u' % m.get_srcSystem()
 
     if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos:
         self.create_vehicle_icon('Sim' + vehicle, 'green')
         self.mpstate.map.set_position('Sim' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))
 
     if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos:
         self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
         self.mpstate.map.set_position('AHRS2' + vehicle, (m.lat*1.0e-7, m.lng*1.0e-7), rotation=math.degrees(m.yaw))
 
     if m.get_type() == "GPS_RAW_INT" and self.map_settings.showgpspos:
         (lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
         if lat != 0 or lon != 0:
             self.create_vehicle_icon('GPS' + vehicle, 'blue')
             self.mpstate.map.set_position('GPS' + vehicle, (lat, lon), rotation=m.cog*0.01)
 
     if m.get_type() == "GPS2_RAW" and self.map_settings.showgps2pos:
         (lat, lon) = (m.lat*1.0e-7, m.lon*1.0e-7)
         if lat != 0 or lon != 0:
             self.create_vehicle_icon('GPS2' + vehicle, 'green')
             self.mpstate.map.set_position('GPS2' + vehicle, (lat, lon), rotation=m.cog*0.01)
 
     if m.get_type() == 'GLOBAL_POSITION_INT':
         (self.lat, self.lon, self.heading) = (m.lat*1.0e-7, m.lon*1.0e-7, m.hdg*0.01)
         if self.lat != 0 or self.lon != 0:
             self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
             self.mpstate.map.set_position('Pos' + vehicle, (self.lat, self.lon), rotation=self.heading)
 
     if m.get_type() == "NAV_CONTROLLER_OUTPUT":
         if self.master.flightmode in [ "AUTO", "GUIDED", "LOITER", "RTL" ]:
             trajectory = [ (self.lat, self.lon),
                            mp_util.gps_newpos(self.lat, self.lon, m.target_bearing, m.wp_dist) ]
             self.mpstate.map.add_object(mp_slipmap.SlipPolygon('trajectory', trajectory, layer='Trajectory',
                                                           linewidth=2, colour=(255,0,180)))
         else:
             self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('Trajectory'))
 
         
     # if the waypoints have changed, redisplay
     if self.wp_change_time != self.module('wp').wploader.last_change:
         self.wp_change_time = self.module('wp').wploader.last_change
         self.display_waypoints()
 
     # if the fence has changed, redisplay
     if self.fence_change_time != self.module('fence').fenceloader.last_change:
         self.display_fence()
 
     # if the rallypoints have changed, redisplay
     if self.rally_change_time != self.module('rally').rallyloader.last_change:
         self.rally_change_time = self.module('rally').rallyloader.last_change
         icon = self.mpstate.map.icon('rallypoint.png')
         self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('RallyPoints'))
         for i in range(self.module('rally').rallyloader.rally_count()):
             rp = self.module('rally').rallyloader.rally_point(i)
             popup = MPMenuSubMenu('Popup',
                                   items=[MPMenuItem('Rally Remove', returnkey='popupRallyRemove'),
                                          MPMenuItem('Rally Move', returnkey='popupRallyMove')])
             self.mpstate.map.add_object(mp_slipmap.SlipIcon('Rally %u' % (i+1), (rp.lat*1.0e-7, rp.lng*1.0e-7), icon,
                                                             layer='RallyPoints', rotation=0, follow=False,
                                                             popup_menu=popup))
 
     # check for any events from the map
     self.mpstate.map.check_events()
 def move(self, heading, distance):
     lat = self.pkt['I105']['Lat']['val']
     lon = self.pkt['I105']['Lon']['val']
     (lat, lon) = mp_util.gps_newpos(lat, lon, heading, distance)
     self.setpos(lat, lon)
Exemple #44
0
def mavflightview_show(path, wp, fen, used_flightmodes, mav_type, options, instances, title=None, timelim_pipe=None):
    if not title:
        title='MAVFlightView'


    boundary_path = []
    for p in path[0]:
        boundary_path.append((p[0],p[1]))

    fence = fen.polygon()
    if options.fencebounds:
        for p in fence:
            boundary_path.append((p[0],p[1]))

    bounds = mp_util.polygon_bounds(boundary_path)
    (lat, lon) = (bounds[0]+bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat-bounds[2], lon+bounds[3])
    while (mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width-20 or
           mp_util.gps_distance(lat, lon, lat, bounds[1]+bounds[3]) >= ground_width-20):
        ground_width += 10

    path_objs = []
    for i in range(len(path)):
        if len(path[i]) != 0:
            path_objs.append(mp_slipmap.SlipPolygon('FlightPath[%u]-%s' % (i,title), path[i], layer='FlightPath',
                                                    linewidth=2, colour=(255,0,180)))
    plist = wp.polygon_list()
    mission_obj = None
    if len(plist) > 0:
        mission_obj = []
        for i in range(len(plist)):
            mission_obj.append(mp_slipmap.SlipPolygon('Mission-%s-%u' % (title,i), plist[i], layer='Mission',
                                                      linewidth=2, colour=(255,255,255)))
    else:
        mission_obj = None

    if len(fence) > 1:
        fence_obj = mp_slipmap.SlipPolygon('Fence-%s' % title, fen.polygon(), layer='Fence',
                                           linewidth=2, colour=(0,255,0))
    else:
        fence_obj = None

    kml = getattr(options,'kml',None)
    if kml is not None:
        kml_objects = load_kml(kml)
    else:
        kml_objects = None

    if options.imagefile:
        create_imagefile(options, options.imagefile, (lat,lon), ground_width, path_objs, mission_obj, fence_obj, kml_objects, used_flightmodes=used_flightmodes, mav_type=mav_type)
    else:
        global multi_map
        if options.multi and multi_map is not None:
            map = multi_map
        else:
            map = mp_slipmap.MPSlipMap(title=title,
                                       service=options.service,
                                       elevation=True,
                                       width=600,
                                       height=600,
                                       ground_width=ground_width,
                                       lat=lat, lon=lon,
                                       debug=options.debug,
                                       show_flightmode_legend=options.show_flightmode_legend,
                                       timelim_pipe=timelim_pipe)
        if options.multi:
            multi_map = map
        for path_obj in path_objs:
            map.add_object(path_obj)
        if mission_obj is not None:
            display_waypoints(wp, map)
        if fence_obj is not None:
            map.add_object(fence_obj)

        if kml_objects is not None:
            for obj in kml_objects:
                map.add_object(obj)
            
        for flag in options.flag:
            a = flag.split(',')
            lat = a[0]
            lon = a[1]
            icon = 'flag.png'
            if len(a) > 2:
                icon = a[2] + '.png'
            icon = map.icon(icon)
            map.add_object(mp_slipmap.SlipIcon('icon - %s' % str(flag), (float(lat),float(lon)), icon, layer=3, rotation=0, follow=False))

        if options.colour_source == "flightmode":
            tuples = [ (mode, colour_for_flightmode(mav_type, mode))
                       for mode in used_flightmodes.keys() ]
            map.add_object(mp_slipmap.SlipFlightModeLegend("legend", tuples))
        elif options.colour_source == "type":
            tuples = [ (t, map_colours[instances[t]]) for t in instances.keys() ]
            map.add_object(mp_slipmap.SlipFlightModeLegend("legend", tuples))
        else:
            print("colour-source: min=%f max=%f" % (colour_source_min, colour_source_max))
Exemple #45
0
def mavflightview(filename):
    print("Loading %s ..." % filename)
    mlog = mavutil.mavlink_connection(filename)
    wp = mavwp.MAVWPLoader()
    if opts.mission is not None:
        wp.load(opts.mission)
    fen = mavwp.MAVFenceLoader()
    if opts.fence is not None:
        fen.load(opts.fence)
    path = [[]]
    instances = {}
    ekf_counter = 0
    types = ["MISSION_ITEM", "CMD"]
    if opts.types is not None:
        types.extend(opts.types.split(","))
    else:
        types.extend(["GPS", "GLOBAL_POSITION_INT"])
        if opts.rawgps or opts.dualgps:
            types.extend(["GPS", "GPS_RAW_INT"])
        if opts.rawgps2 or opts.dualgps:
            types.extend(["GPS2_RAW", "GPS2"])
        if opts.ekf:
            types.extend(["EKF1", "GPS"])
        if opts.ahr2:
            types.extend(["AHR2", "AHRS2", "GPS"])
    print("Looking for types %s" % str(types))

    last_timestamps = {}

    while True:
        try:
            m = mlog.recv_match(type=types)
            if m is None:
                break
        except Exception:
            break

        type = m.get_type()

        if type == "MISSION_ITEM":
            try:
                while m.seq > wp.count():
                    print("Adding dummy WP %u" % wp.count())
                    wp.set(m, wp.count())
                wp.set(m, m.seq)
            except Exception:
                pass
            continue
        if type == "CMD":
            m = mavutil.mavlink.MAVLink_mission_item_message(
                0,
                0,
                m.CNum,
                mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
                m.CId,
                0,
                1,
                m.Prm1,
                m.Prm2,
                m.Prm3,
                m.Prm4,
                m.Lat,
                m.Lng,
                m.Alt,
            )
            try:
                while m.seq > wp.count():
                    print("Adding dummy WP %u" % wp.count())
                    wp.set(m, wp.count())
                wp.set(m, m.seq)
            except Exception:
                pass
            continue
        if not mlog.check_condition(opts.condition):
            continue
        if opts.mode is not None and mlog.flightmode.lower() != opts.mode.lower():
            continue
        if type in ["GPS", "GPS2"]:
            status = getattr(m, "Status", None)
            if status is None:
                status = getattr(m, "FixType", None)
                if status is None:
                    print("Can't find status on GPS message")
                    print(m)
                    break
            if status < 2:
                continue
            # flash log
            lat = m.Lat
            lng = getattr(m, "Lng", None)
            if lng is None:
                lng = getattr(m, "Lon", None)
                if lng is None:
                    print("Can't find longitude on GPS message")
                    print(m)
                    break
        elif type in ["EKF1", "ANU1"]:
            pos = mavextra.ekf1_pos(m)
            if pos is None:
                continue
            ekf_counter += 1
            if ekf_counter % opts.ekf_sample != 0:
                continue
            (lat, lng) = pos
        elif type in ["ANU5"]:
            (lat, lng) = (m.Alat * 1.0e-7, m.Alng * 1.0e-7)
        elif type in ["AHR2", "POS", "CHEK"]:
            (lat, lng) = (m.Lat, m.Lng)
        elif type == "AHRS2":
            (lat, lng) = (m.lat * 1.0e-7, m.lng * 1.0e-7)
        else:
            lat = m.lat * 1.0e-7
            lng = m.lon * 1.0e-7

        # automatically add new types to instances
        if type not in instances:
            instances[type] = len(instances)
            while len(instances) >= len(path):
                path.append([])
        instance = instances[type]

        if abs(lat) > 0.01 or abs(lng) > 0.01:
            fmode = getattr(mlog, "flightmode", "")
            if fmode in colourmap:
                colour = colourmap[fmode]
            else:
                colour = colourmap["UNKNOWN"]
            (r, g, b) = colour
            (r, g, b) = (r + instance * 80, g + instance * 50, b + instance * 70)
            if r > 255:
                r = 205
            if g > 255:
                g = 205
            if g < 0:
                g = 0
            if b > 255:
                b = 205
            colour = (r, g, b)
            point = (lat, lng, colour)

            if opts.rate == 0 or not type in last_timestamps or m._timestamp - last_timestamps[type] > 1.0 / opts.rate:
                last_timestamps[type] = m._timestamp
                path[instance].append(point)
    if len(path[0]) == 0:
        print("No points to plot")
        return
    bounds = mp_util.polygon_bounds(path[0])
    (lat, lon) = (bounds[0] + bounds[2], bounds[1])
    (lat, lon) = mp_util.gps_newpos(lat, lon, -45, 50)
    ground_width = mp_util.gps_distance(lat, lon, lat - bounds[2], lon + bounds[3])
    while (
        mp_util.gps_distance(lat, lon, bounds[0], bounds[1]) >= ground_width - 20
        or mp_util.gps_distance(lat, lon, lat, bounds[1] + bounds[3]) >= ground_width - 20
    ):
        ground_width += 10

    path_objs = []
    for i in range(len(path)):
        if len(path[i]) != 0:
            path_objs.append(
                mp_slipmap.SlipPolygon(
                    "FlightPath[%u]-%s" % (i, filename), path[i], layer="FlightPath", linewidth=2, colour=(255, 0, 180)
                )
            )
    plist = wp.polygon_list()
    mission_obj = None
    if len(plist) > 0:
        mission_obj = []
        for i in range(len(plist)):
            mission_obj.append(
                mp_slipmap.SlipPolygon(
                    "Mission-%s-%u" % (filename, i), plist[i], layer="Mission", linewidth=2, colour=(255, 255, 255)
                )
            )
    else:
        mission_obj = None

    fence = fen.polygon()
    if len(fence) > 1:
        fence_obj = mp_slipmap.SlipPolygon(
            "Fence-%s" % filename, fen.polygon(), layer="Fence", linewidth=2, colour=(0, 255, 0)
        )
    else:
        fence_obj = None

    if opts.imagefile:
        create_imagefile(opts.imagefile, (lat, lon), ground_width, path_objs, mission_obj, fence_obj)
    else:
        global multi_map
        if opts.multi and multi_map is not None:
            map = multi_map
        else:
            map = mp_slipmap.MPSlipMap(
                title=filename,
                service=opts.service,
                elevation=True,
                width=600,
                height=600,
                ground_width=ground_width,
                lat=lat,
                lon=lon,
                debug=opts.debug,
            )
        if opts.multi:
            multi_map = map
        for path_obj in path_objs:
            map.add_object(path_obj)
        if mission_obj is not None:
            display_waypoints(wp, map)
        if fence_obj is not None:
            map.add_object(fence_obj)

        for flag in opts.flag:
            a = flag.split(",")
            lat = a[0]
            lon = a[1]
            icon = "flag.png"
            if len(a) > 2:
                icon = a[2] + ".png"
            icon = map.icon(icon)
            map.add_object(
                mp_slipmap.SlipIcon(
                    "icon - %s" % str(flag), (float(lat), float(lon)), icon, layer=3, rotation=0, follow=False
                )
            )
Exemple #46
0
    def mavlink_packet(self, m):
        '''handle an incoming mavlink packet'''
        if m.get_type() == "HEARTBEAT":
            from pymavlink import mavutil
            if m.type in [mavutil.mavlink.MAV_TYPE_FIXED_WING]:
                self.vehicle_type_name = 'plane'
            elif m.type in [
                    mavutil.mavlink.MAV_TYPE_GROUND_ROVER,
                    mavutil.mavlink.MAV_TYPE_SURFACE_BOAT,
                    mavutil.mavlink.MAV_TYPE_SUBMARINE
            ]:
                self.vehicle_type_name = 'rover'
            elif m.type in [
                    mavutil.mavlink.MAV_TYPE_QUADROTOR,
                    mavutil.mavlink.MAV_TYPE_COAXIAL,
                    mavutil.mavlink.MAV_TYPE_HEXAROTOR,
                    mavutil.mavlink.MAV_TYPE_OCTOROTOR,
                    mavutil.mavlink.MAV_TYPE_TRICOPTER,
                    mavutil.mavlink.MAV_TYPE_HELICOPTER
            ]:
                self.vehicle_type_name = 'copter'
            elif m.type in [mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER]:
                self.vehicle_type_name = 'antenna'

        # this is the beginnings of allowing support for multiple vehicles
        # in the air at the same time
        vehicle = 'Vehicle%u' % m.get_srcSystem()

        if m.get_type() == "SIMSTATE" and self.map_settings.showsimpos:
            self.create_vehicle_icon('Sim' + vehicle, 'green')
            self.mpstate.map.set_position('Sim' + vehicle,
                                          (m.lat * 1.0e-7, m.lng * 1.0e-7),
                                          rotation=math.degrees(m.yaw))

        if m.get_type() == "AHRS2" and self.map_settings.showahrs2pos:
            self.create_vehicle_icon('AHRS2' + vehicle, 'blue')
            self.mpstate.map.set_position('AHRS2' + vehicle,
                                          (m.lat * 1.0e-7, m.lng * 1.0e-7),
                                          rotation=math.degrees(m.yaw))

        if m.get_type() == "GPS_RAW_INT" and self.map_settings.showgpspos:
            (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
            if lat != 0 or lon != 0:
                self.create_vehicle_icon('GPS' + vehicle, 'blue')
                self.mpstate.map.set_position('GPS' + vehicle, (lat, lon),
                                              rotation=m.cog * 0.01)

        if m.get_type() == "GPS2_RAW" and self.map_settings.showgps2pos:
            (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
            if lat != 0 or lon != 0:
                self.create_vehicle_icon('GPS2' + vehicle, 'green')
                self.mpstate.map.set_position('GPS2' + vehicle, (lat, lon),
                                              rotation=m.cog * 0.01)

        if m.get_type() == 'GLOBAL_POSITION_INT':
            (self.lat, self.lon, self.heading) = (m.lat * 1.0e-7,
                                                  m.lon * 1.0e-7, m.hdg * 0.01)
            if self.lat != 0 or self.lon != 0:
                self.create_vehicle_icon('Pos' + vehicle, 'red', follow=True)
                self.mpstate.map.set_position('Pos' + vehicle,
                                              (self.lat, self.lon),
                                              rotation=self.heading)

        if m.get_type() == "NAV_CONTROLLER_OUTPUT":
            if self.master.flightmode in ["AUTO", "GUIDED", "LOITER", "RTL"]:
                trajectory = [(self.lat, self.lon),
                              mp_util.gps_newpos(self.lat, self.lon,
                                                 m.target_bearing, m.wp_dist)]
                self.mpstate.map.add_object(
                    mp_slipmap.SlipPolygon('trajectory',
                                           trajectory,
                                           layer='Trajectory',
                                           linewidth=2,
                                           colour=(255, 0, 180)))
            else:
                self.mpstate.map.add_object(
                    mp_slipmap.SlipClearLayer('Trajectory'))

        # if the waypoints have changed, redisplay
        last_wp_change = self.module('wp').wploader.last_change
        if self.wp_change_time != last_wp_change and abs(time.time() -
                                                         last_wp_change) > 1:
            self.wp_change_time = last_wp_change
            self.display_waypoints()

            #this may have affected the landing lines from the rally points:
            self.rally_change_time = time.time()

        # if the fence has changed, redisplay
        if self.fence_change_time != self.module(
                'fence').fenceloader.last_change:
            self.display_fence()

        # if the rallypoints have changed, redisplay
        if self.rally_change_time != self.module(
                'rally').rallyloader.last_change:
            self.rally_change_time = self.module(
                'rally').rallyloader.last_change
            icon = self.mpstate.map.icon('rallypoint.png')
            self.mpstate.map.add_object(
                mp_slipmap.SlipClearLayer('RallyPoints'))
            for i in range(self.module('rally').rallyloader.rally_count()):
                rp = self.module('rally').rallyloader.rally_point(i)
                popup = MPMenuSubMenu(
                    'Popup',
                    items=[
                        MPMenuItem('Rally Remove',
                                   returnkey='popupRallyRemove'),
                        MPMenuItem('Rally Move', returnkey='popupRallyMove')
                    ])
                self.mpstate.map.add_object(
                    mp_slipmap.SlipIcon('Rally %u' % (i + 1),
                                        (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
                                        icon,
                                        layer='RallyPoints',
                                        rotation=0,
                                        follow=False,
                                        popup_menu=popup))

                loiter_rad = self.get_mav_param('WP_LOITER_RAD')

                if self.map_settings.rallycircle:
                    self.mpstate.map.add_object(
                        mp_slipmap.SlipCircle(
                            'Rally Circ %u' % (i + 1),
                            'RallyPoints', (rp.lat * 1.0e-7, rp.lng * 1.0e-7),
                            abs(loiter_rad), (255, 255, 0), 2))

                #draw a line between rally point and nearest landing point
                nearest_land_wp = None
                nearest_distance = 10000000.0
                for j in range(self.module('wp').wploader.count()):
                    w = self.module('wp').wploader.wp(j)
                    if (w.command == 21):  #if landing waypoint
                        #get distance between rally point and this waypoint
                        dis = mp_util.gps_distance(w.x, w.y, rp.lat * 1.0e-7,
                                                   rp.lng * 1.0e-7)
                        if (dis < nearest_distance):
                            nearest_land_wp = w
                            nearest_distance = dis

                if nearest_land_wp != None:
                    points = []
                    #tangential approach?
                    if self.get_mav_param('LAND_BREAK_PATH') == 0:
                        theta = math.degrees(
                            math.atan(loiter_rad / nearest_distance))
                        tan_dis = math.sqrt(nearest_distance *
                                            nearest_distance -
                                            (loiter_rad * loiter_rad))

                        ral_bearing = mp_util.gps_bearing(
                            nearest_land_wp.x, nearest_land_wp.y,
                            rp.lat * 1.0e-7, rp.lng * 1.0e-7)

                        points.append(
                            mp_util.gps_newpos(nearest_land_wp.x,
                                               nearest_land_wp.y,
                                               ral_bearing + theta, tan_dis))

                    else:  #not tangential approach
                        points.append((rp.lat * 1.0e-7, rp.lng * 1.0e-7))

                    points.append((nearest_land_wp.x, nearest_land_wp.y))
                    self.mpstate.map.add_object(
                        mp_slipmap.SlipPolygon('Rally Land %u' % (i + 1),
                                               points, 'RallyPoints',
                                               (255, 255, 0), 2))

        # check for any events from the map
        self.mpstate.map.check_events()
Exemple #47
0
def mavlink_packet(m):
    '''handle an incoming mavlink packet'''
    state = mpstate.map_state

    if m.get_type() == "SIMSTATE":
        if not mpstate.map_state.have_simstate:
            mpstate.map_state.have_simstate = True
            create_blueplane()
        mpstate.map.set_position('blueplane', (m.lat * 1.0e-7, m.lng * 1.0e-7),
                                 rotation=math.degrees(m.yaw))

    if m.get_type() == "GPS_RAW_INT" and not mpstate.map_state.have_simstate:
        (lat, lon) = (m.lat * 1.0e-7, m.lon * 1.0e-7)
        if state.lat is not None and (
                mpstate.map_state.have_blueplane
                or mp_util.gps_distance(lat, lon, state.lat, state.lon) > 10):
            create_blueplane()
            mpstate.map.set_position('blueplane', (lat, lon),
                                     rotation=m.cog * 0.01)

    if m.get_type() == "NAV_CONTROLLER_OUTPUT":
        if mpstate.master().flightmode in ["AUTO", "GUIDED", "LOITER", "RTL"]:
            trajectory = [(state.lat, state.lon),
                          mp_util.gps_newpos(state.lat, state.lon,
                                             m.target_bearing, m.wp_dist)]
            mpstate.map.add_object(
                mp_slipmap.SlipPolygon('trajectory',
                                       trajectory,
                                       layer='Trajectory',
                                       linewidth=2,
                                       colour=(255, 0, 180)))
        else:
            mpstate.map.add_object(mp_slipmap.SlipClearLayer('Trajectory'))

    if m.get_type() == 'GLOBAL_POSITION_INT':
        (state.lat, state.lon, state.heading) = (m.lat * 1.0e-7,
                                                 m.lon * 1.0e-7, m.hdg * 0.01)
    else:
        return

    if state.lat != 0 or state.lon != 0:
        mpstate.map.set_position('plane', (state.lat, state.lon),
                                 rotation=state.heading)

    # if the waypoints have changed, redisplay
    if state.wp_change_time != mpstate.status.wploader.last_change:
        state.wp_change_time = mpstate.status.wploader.last_change
        display_waypoints()

    # if the fence has changed, redisplay
    if state.fence_change_time != mpstate.status.fenceloader.last_change:
        state.fence_change_time = mpstate.status.fenceloader.last_change
        points = mpstate.status.fenceloader.polygon()
        if len(points) > 1:
            mpstate.map.add_object(
                mp_slipmap.SlipPolygon('fence',
                                       points,
                                       layer=1,
                                       linewidth=2,
                                       colour=(0, 255, 0)))

    # check for any events from the map
    mpstate.map.check_events()