Beispiel #1
0
 def update_position(self):
     '''update position text'''
     state = self.state
     pos = self.mouse_pos
     newtext = ''
     alt = 0
     if pos is not None:
         (lat,lon) = self.coordinates(pos.x, pos.y)
         newtext += 'Cursor: %f %f (%s)' % (lat, lon, mp_util.latlon_to_grid((lat, lon)))
         if state.elevation:
             alt = self.ElevationMap.GetElevation(lat, lon)
             newtext += ' %.1fm' % alt
     pending = state.mt.tiles_pending()
     if pending:
         newtext += ' Map Downloading %u ' % pending
     if alt == -1:
         newtext += ' SRTM Downloading '
     newtext += '\n'
     if self.click_pos is not None:
         newtext += 'Click: %f %f (%s %s) (%s)' % (self.click_pos[0], self.click_pos[1],
                                                   mp_util.degrees_to_dms(self.click_pos[0]),
                                                   mp_util.degrees_to_dms(self.click_pos[1]),
                                                   mp_util.latlon_to_grid(self.click_pos))
     if self.last_click_pos is not None:
         distance = mp_util.gps_distance(self.last_click_pos[0], self.last_click_pos[1],
                                         self.click_pos[0], self.click_pos[1])
         bearing = mp_util.gps_bearing(self.last_click_pos[0], self.last_click_pos[1],
                                         self.click_pos[0], self.click_pos[1])
         newtext += '  Distance: %.1fm Bearing %.1f' % (distance, bearing)
     if newtext != state.oldtext:
         self.position.Clear()
         self.position.WriteText(newtext)
         state.oldtext = newtext
Beispiel #2
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)
Beispiel #3
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)
Beispiel #4
0
 def update_position(self):
     '''update position text'''
     state = self.state
     pos = self.mouse_pos
     newtext = ''
     alt = 0
     if pos is not None:
         (lat, lon) = self.coordinates(pos.x, pos.y)
         newtext += 'Cursor: %.8f %.8f (%s)' % (lat, lon,
                                                mp_util.latlon_to_grid(
                                                    (lat, lon)))
         if state.elevation:
             alt = self.ElevationMap.GetElevation(lat, lon)
             if alt is not None:
                 newtext += ' %.1fm %uft' % (alt, alt * 3.28084)
     state.mt.set_download(state.download)
     pending = 0
     if state.download:
         pending = state.mt.tiles_pending()
     if pending:
         newtext += ' Map Downloading %u ' % pending
     if alt == -1:
         newtext += ' SRTM Downloading '
     newtext += '\n'
     if self.click_pos is not None:
         newtext += 'Click: %.8f %.8f (%s %s) (%s)' % (
             self.click_pos[0], self.click_pos[1],
             mp_util.degrees_to_dms(self.click_pos[0]),
             mp_util.degrees_to_dms(
                 self.click_pos[1]), mp_util.latlon_to_grid(self.click_pos))
     if self.last_click_pos is not None:
         distance = mp_util.gps_distance(self.last_click_pos[0],
                                         self.last_click_pos[1],
                                         self.click_pos[0],
                                         self.click_pos[1])
         bearing = mp_util.gps_bearing(self.last_click_pos[0],
                                       self.last_click_pos[1],
                                       self.click_pos[0], self.click_pos[1])
         newtext += '  Distance: %.3fm %.3fnm Bearing %.1f' % (
             distance, distance * 0.000539957, bearing)
     if newtext != state.oldtext:
         self.position.Clear()
         self.position.WriteText(newtext)
         state.oldtext = newtext
    def mavlink_packet(self, m):
        '''handle an incoming mavlink packet'''
        if not self.mpstate.map:
            # don't draw if no map
            return

        if m.get_type() != 'GLOBAL_POSITION_INT':
            return
        self.update_target(m.time_boot_ms)

        if self.target_pos is None:
            return

        if self.follow_settings.disable_msg:
            return

        if self.follow_settings.type == 'guided':
            # send normal guided mode packet
            self.master.mav.mission_item_send(self.settings.target_system,
                                              self.settings.target_component,
                                              0,
                                              self.module('wp').get_default_frame(),
                                              mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
                                              2, 0, 0, 0, 0, 0,
                                              self.target_pos[0], self.target_pos[1],
                                              self.follow_settings.altitude)

        elif self.follow_settings.type == 'yaw':
            # display yaw from vehicle to target
            vehicle = (m.lat*1.0e-7, m.lon*1.0e-7)
            vehicle_yaw = math.degrees(self.master.field('ATTITUDE', 'yaw', 0))
            target_bearing = mp_util.gps_bearing(vehicle[0], vehicle[1], self.target_pos[0], self.target_pos[1])
            # wrap the angle from -180 to 180 thus commanding the vehicle to turn left or right
            # note its in centi-degrees so *100
            relyaw = self.wrap_180(target_bearing - vehicle_yaw) * 100

            self.master.mav.command_long_send(self.settings.target_system,
                                                  self.settings.target_component,
                                                  mavutil.mavlink.MAV_CMD_NAV_SET_YAW_SPEED, 0,
                                                  relyaw,
                                                  self.follow_settings.vehicle_throttle,
                                                  0, 0, 0, 0, 0)
Beispiel #6
0
 def update_position(self):
     """update position text"""
     state = self.state
     pos = self.mouse_pos
     newtext = ""
     alt = 0
     if pos is not None:
         (lat, lon) = self.coordinates(pos.x, pos.y)
         newtext += "Cursor: %f %f (%s)" % (lat, lon, mp_util.latlon_to_grid((lat, lon)))
         if state.elevation:
             alt = self.ElevationMap.GetElevation(lat, lon)
             if alt is not None:
                 newtext += " %.1fm" % alt
     state.mt.set_download(state.download)
     pending = 0
     if state.download:
         pending = state.mt.tiles_pending()
     if pending:
         newtext += " Map Downloading %u " % pending
     if alt == -1:
         newtext += " SRTM Downloading "
     newtext += "\n"
     if self.click_pos is not None:
         newtext += "Click: %f %f (%s %s) (%s)" % (
             self.click_pos[0],
             self.click_pos[1],
             mp_util.degrees_to_dms(self.click_pos[0]),
             mp_util.degrees_to_dms(self.click_pos[1]),
             mp_util.latlon_to_grid(self.click_pos),
         )
     if self.last_click_pos is not None:
         distance = mp_util.gps_distance(
             self.last_click_pos[0], self.last_click_pos[1], self.click_pos[0], self.click_pos[1]
         )
         bearing = mp_util.gps_bearing(
             self.last_click_pos[0], self.last_click_pos[1], self.click_pos[0], self.click_pos[1]
         )
         newtext += "  Distance: %.1fm Bearing %.1f" % (distance, bearing)
     if newtext != state.oldtext:
         self.position.Clear()
         self.position.WriteText(newtext)
         state.oldtext = newtext
Beispiel #7
0
    def mavlink_packet(self, m):
        '''handle an incoming mavlink packet'''
        if not self.mpstate.map:
            # don't draw if no map
            return

        if m.get_type() != 'GLOBAL_POSITION_INT':
            return
        self.update_target(m.time_boot_ms)

        if self.target_pos is None:
            return

        if self.follow_settings.disable_msg:
            return

        if self.follow_settings.type == 'guided':
            # send normal guided mode packet
            for m, t, c in self.master:
                m.mav.mission_item_int_send(
                    t, c, 0,
                    self.module('wp').get_default_frame(),
                    mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2, 0, 0, 0, 0, 0,
                    int(self.target_pos[0] * 1.0e7),
                    int(self.target_pos[1] * 1.0e7),
                    self.follow_settings.altitude)

        elif self.follow_settings.type == 'yaw':
            # display yaw from vehicle to target
            vehicle = (m.lat * 1.0e-7, m.lon * 1.0e-7)
            vehicle_yaw = math.degrees(self.master.field('ATTITUDE', 'yaw', 0))
            target_bearing = mp_util.gps_bearing(vehicle[0], vehicle[1],
                                                 self.target_pos[0],
                                                 self.target_pos[1])
            # wrap the angle from -180 to 180 thus commanding the vehicle to turn left or right
            # note its in centi-degrees so *100
            relyaw = self.wrap_180(target_bearing - vehicle_yaw) * 100

            for m, t, c in self.master:
                m.mav.command_long_send(
                    t, c, mavutil.mavlink.MAV_CMD_NAV_SET_YAW_SPEED, 0, relyaw,
                    self.follow_settings.vehicle_throttle, 0, 0, 0, 0, 0)
Beispiel #8
0
 def update(self, pkt, tnow):
     dt = tnow - self.last_time
     if dt < 0 or dt > 10:
         self.last_time = tnow
         return
     if dt < 0.1:
         return
     self.last_time = tnow
     dist = mp_util.gps_distance(self.pkt.lat*1e-7, self.pkt.lon*1e-7,
                                 pkt.lat*1e-7, pkt.lon*1e-7)
     if dist > 0.01:
         heading = mp_util.gps_bearing(self.pkt.lat*1e-7, self.pkt.lon*1e-7,
                                     pkt.lat*1e-7, pkt.lon*1e-7)
         spd = dist / dt
         pkt.heading = int(heading*100)
         new_vel = int(spd * 100)
         #print(pkt.ICAO_address, new_vel*0.01, pkt.hor_velocity*0.01)
         pkt.hor_velocity = new_vel
     if pkt.hor_velocity > 65535:
         pkt.hor_velocity = 65535
     self.pkt = pkt
Beispiel #9
0
    def run(self):
        '''Main command loop for the mission'''
        try:

            print("Traveling to: %s\n" % (str(self.next_wp)))

            self.predive_check(
            )  # checks that auv is ready to dive, throws exception if not

            # Distance to the next waypoint
            self.distance_to_waypoint = mp_util.gps_distance(
                self.lat, self.lon, self.next_wp[0], self.next_wp[1])

            # Offset in degrees from the direction of the next waypoint
            self.offset_from_intended_heading = mp_util.gps_bearing(
                self.lat, self.lon, self.next_wp[0], self.next_wp[1])

            # Spins the AUV towards the correct direction
            self.orient_heading(self.offset_from_intended_heading)

            # self.dive()

            self.underwater_traverse(self.distance_to_waypoint)

            # self.surface()

            # Tells this function to run again, this implements the loop functionality
            self.continue_mission()

        except HardwareError:
            print(HardwareError.message)
            print('Hardware Error')
            return self.go_home()

        except ValueError:
            print(ValueError)
            print('Value Error')
            return self.go_home()

        return
 def update(self, pkt, tnow):
     dt = tnow - self.last_time
     if dt < 0 or dt > 10:
         self.last_time = tnow
         return
     if dt < 0.1:
         return
     self.last_time = tnow
     dist = mp_util.gps_distance(self.pkt.lat * 1e-7, self.pkt.lon * 1e-7,
                                 pkt.lat * 1e-7, pkt.lon * 1e-7)
     if dist > 0.01:
         heading = mp_util.gps_bearing(self.pkt.lat * 1e-7,
                                       self.pkt.lon * 1e-7, pkt.lat * 1e-7,
                                       pkt.lon * 1e-7)
         spd = dist / dt
         pkt.heading = int(heading * 100)
         new_vel = int(spd * 100)
         #print(pkt.ICAO_address, new_vel*0.01, pkt.hor_velocity*0.01)
         pkt.hor_velocity = new_vel
     if pkt.hor_velocity > 65535:
         pkt.hor_velocity = 65535
     self.pkt = pkt
Beispiel #11
0
    def coord_to_pixel(self, lat, lon, width, ground_width, lat2, lon2):
        '''return pixel coordinate (px,py) for position (lat2,lon2)
        in an area image. Note that the results are relative to top,left
        and may be outside the image
        ground_width is with at lat,lon
        px is pixel coord to the right from top,left
        py is pixel coord down from top left
        '''

        pixel_width_equator = (ground_width / float(width)) / cos(radians(lat))
        latr = radians(lat)
        lat2r = radians(lat2)

        C = mp_util.radius_of_earth / pixel_width_equator
        y = C * (log(abs(1.0/cos(latr) + tan(latr))) - log(abs(1.0/cos(lat2r) + tan(lat2r))))
        y = int(y+0.5)

        dx = mp_util.gps_distance(lat2, lon, lat2, lon2)
        if mp_util.gps_bearing(lat2, lon, lat2, lon2) > 180:
            dx = -dx
        x = int(0.5 + dx / (pixel_width_equator * cos(radians(lat2))))
        return (x,y)
Beispiel #12
0
    def go_home(self):
        '''returns to home coordinate'''
        print('Returning Home\n')
        self.module('rc').override = [
            1500, 1700, 1500, 1500, 1500, 1500, 1500, 1500
        ]
        self.module('rc').send_rc_override()

        sleep(2)
        # Distance to the next waypoint
        self.distance_to_waypoint = mp_util.gps_distance(
            self.lat, self.lon, self.home[lat], self.home[lng])

        # Offset in degrees from the direction of the next waypoint
        self.offset_from_intended_heading = mp_util.gps_bearing(
            self.lat, self.lon, self.home[lat], self.home[lng])

        # Spins the AUV towards the correct direction
        self.orient_heading(self.offset_from_intended_heading)

        self.module('rc').stop()
        self.command_queue.clear()
        self.end_time = 0
        self.command_queue.append(["f", 1650, self.distance_to_waypoint])
Beispiel #13
0
    def cmd_underwater(self):

        mav.set_mode_manual()
        rc.set_mode_manual()
        mav.motors_armed_wait()
        #set the apm mav_type
        mav.mode_mapping()

        if self.predive_check() is not True:
            return "Insufficient Battery"

        self.distance_to_waypoint = mp_util.gps_distance(
            self.lat, self.lon, self.next_wp.MAVLink_mission_item_message.x,
            self.next_wp.MAVLink_mission_item_message.y)

        self.intended_heading = mp_util.gps_bearing(
            self.lat, self.lon, self.next_wp.MAVLink_mission_item_message.x,
            self.next_wp.MAVLink_mission_item_message.y)

        self.orient_heading(self.intended_heading)

        self.pollution_array = None

        #x = lat, y = lng
        self.dive([self.lng, self.lat], [
            self.next_wp.MAVLink_mission_item_message.y,
            self.next_wp.MAVLink_mission_item_message.x
        ], self.distance_to_waypoint, self.intended_heading, 1, 1)

        self.underwater_traverse([self.lng, self.lat], [
            self.next_wp.MAVLink_mission_item_message.y,
            self.next_wp.MAVLink_mission_item_message.x
        ], self.distance_to_waypoint, heading)

        self.surface()
        return
Beispiel #14
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()
Beispiel #15
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()
Beispiel #16
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))
Beispiel #17
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()
Beispiel #18
0
    def idle_task(self):
        '''time motor events, track battery usage, monitor system, and time sensor readings'''
        now = time()

        # try:
        #     self.hardware_check()
        #
        # except PWMError:
        #     print(PWMError.message)
        #     print ('disarminggggggggggggg')
        #     self.module('arm').disarm('force')
        #
        # except HardwareError:
        #     print('errrorrrrr')
        #     if HardwareError.errno is 2:
        #         print(2)
        #         #print(HardwareError.message)
        #         #self.go_home()
        #     elif HardwareError.errno is 3:
        #         print(3)
        #         #self.surface_and_disarm(HardwareError.message)
        #     elif HardwareError.errno is 4:
        #         print(4)
        #         #self.surface_and_disarm(HardwareError.message)
        #     elif HardwareError.errno is 0:
        #print(0)
        #self.surface_and_disarm(HardwareError.message)  # try to surface
        #self.mav.reboot_autopilot()
        #system('sudo reboot now')

        if self.module('rc').override_period.trigger():
            if (self.module('rc').override != [1500] * 16
                    or self.module('rc').override !=
                    self.module('rc').last_override
                    or self.module('rc').override_counter > 0):
                self.module('rc').last_override = self.module('rc').override[:]
                self.module('rc').send_rc_override()
                if self.module('rc').override_counter > 0:
                    self.module('rc').override_counter -= 1

        if self.orienting is True and now - self.last_orient_check > 5:
            self.module('rc').stop()
            sleep(1)
            self.end_time = 0
            if mp_util.gps_bearing(self.lat, self.lon, self.next_wp[0],
                                   self.next_wp[1]) <= 500:
                self.orienting = False
            else:
                self.command_queue.appendleft(['y', 1520, 10])

        if self.reached_wp:
            self.module('rc').stop()
            self.write_to_battery.append(
                "uJoules: %s, Run time: %s, Time: %s \n" %
                (self.ujoules, self.motor_run_time, strftime("%H:%M:%S")))
            self.cmd_move(["continue_mission", 1500, 0])

        # extremely time critical, as the code must send the next 'rc' cmd before the 3 second disarm timeout
        if self.end_time <= time():
            self.module('rc').stop()  # this resets the disarm timer
            # * Code between asterisks must execute in under 3 seconds to keep auv armed
            self.write_to_battery.append(
                "{'uJoules': %s, 'Run time': %s, 'Time': %s}\n" %
                (self.ujoules, self.motor_run_time, strftime("%H:%M:%S")))
            try:
                command = self.command_queue.popleft()
                self.cmd_move(command)
                self.end_time = time() + command[2]
                self.motor_run_time = command[2]
                self.ujoules = 0
            # *
            except IndexError:
                self.end_time = time() + 1
                with open("/home/pi/motor_battery.txt", "a+") as f:
                    f.write(''.join(self.write_to_battery))
                self.write_to_battery = []

        if now - self.last_batt >= 1:
            self.last_batt = now
            self.ujoules += self.batt_info()

        if now - self.last_sample > 1:
            self.last_sample = now
            # * Code between asterisks must execute in under 3 seconds to keep auv armed
            if self.sample() and self.dense is False:
                print(
                    'Pollution information exceeding threshold, starting dense traversal\n'
                )
                self.command_queue.append([
                    "end_dense", 1600,
                    (self.end_time - self.dense_traverse() -
                     (time() - self.start_time))
                ])
                self.surface()
        # *
        return
Beispiel #19
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()
Beispiel #20
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))