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
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 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)
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
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)
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
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
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)
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])
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
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()
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()
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))
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()
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
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()
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))