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