def nofly_add(self):
'''add a square flight exclusion zone'''
latlon = self.mpstate.click_location
if latlon is None:
print("No position chosen")
return
loader = self.wploader
(center_lat, center_lon) = latlon
points = []
points.append(mp_util.gps_offset(center_lat, center_lon, -25, 25))
points.append(mp_util.gps_offset(center_lat, center_lon, 25, 25))
points.append(mp_util.gps_offset(center_lat, center_lon, 25, -25))
points.append(mp_util.gps_offset(center_lat, center_lon, -25, -25))
start_idx = loader.count()
for p in points:
wp = mavutil.mavlink.MAVLink_mission_item_message(
0, 0, 0, 0,
mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0,
1, 4, 0, 0, 0, p[0], p[1], 0)
loader.add(wp)
self.loading_waypoints = True
self.loading_waypoint_lasttime = time.time()
self.master.mav.mission_write_partial_list_send(
self.target_system, self.target_component, start_idx,
start_idx + 4)
print("Added nofly zone")
def coord_from_area(self, x, y, lat, lon, width, ground_width):
'''return (lat,lon) for a pixel in an area image'''
pixel_width = ground_width / float(width)
dx = x * pixel_width
dy = y * pixel_width
return mp_util.gps_offset(lat, lon, dx, -dy)
def coord_from_area(self, x, y, lat, lon, width, ground_width): '''return (lat,lon) for a pixel in an area image''' pixel_width = ground_width / float(width) dx = x * pixel_width dy = y * pixel_width return mp_util.gps_offset(lat, lon, dx, -dy)
def send_terrain_data_bit(self, bit):
'''send some terrain data'''
lat = self.current_request.lat * 1.0e-7
lon = self.current_request.lon * 1.0e-7
bit_spacing = self.current_request.grid_spacing * 4
(lat, lon) = mp_util.gps_offset(lat,
lon,
east=bit_spacing * (bit % 8),
north=bit_spacing * (bit // 8))
data = []
for i in range(4 * 4):
y = i % 4
x = i // 4
(lat2, lon2) = mp_util.gps_offset(
lat,
lon,
east=self.current_request.grid_spacing * y,
north=self.current_request.grid_spacing * x)
alt = self.ElevationModel.GetElevation(lat2, lon2)
if alt is None:
if self.terrain_settings.debug:
print("no alt ", lat2, lon2)
return
data.append(int(alt))
self.master.mav.terrain_data_send(self.current_request.lat,
self.current_request.lon,
self.current_request.grid_spacing,
bit, data)
self.blocks_sent += 1
self.last_send_time = time.time()
self.sent_mask |= 1 << bit
if self.terrain_settings.debug and bit == 55:
lat = self.current_request.lat * 1.0e-7
lon = self.current_request.lon * 1.0e-7
print("--lat=%f --lon=%f %.1f" %
(lat, lon, self.ElevationModel.GetElevation(lat, lon)))
(lat2, lon2) = mp_util.gps_offset(
lat,
lon,
east=32 * self.current_request.grid_spacing,
north=28 * self.current_request.grid_spacing)
print("--lat=%f --lon=%f %.1f" %
(lat2, lon2, self.ElevationModel.GetElevation(lat2, lon2)))
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() != 'GIMBAL_REPORT':
return
needed = ['ATTITUDE', 'GLOBAL_POSITION_INT']
for n in needed:
if not n in self.master.messages:
return
# clear the camera icon
self.mpstate.map.add_object(mp_slipmap.SlipClearLayer('GimbalView'))
gpi = self.master.messages['GLOBAL_POSITION_INT']
att = self.master.messages['ATTITUDE']
vehicle_dcm = Matrix3()
vehicle_dcm.from_euler(att.roll, att.pitch, att.yaw)
rotmat_copter_gimbal = Matrix3()
rotmat_copter_gimbal.from_euler312(m.joint_roll, m.joint_el,
m.joint_az)
gimbal_dcm = vehicle_dcm * rotmat_copter_gimbal
lat = gpi.lat * 1.0e-7
lon = gpi.lon * 1.0e-7
alt = gpi.relative_alt * 1.0e-3
# ground plane
ground_plane = Plane()
# the position of the camera in the air, remembering its a right
# hand coordinate system, so +ve z is down
camera_point = Vector3(0, 0, -alt)
# get view point of camera when not rotated
view_point = Vector3(1, 0, 0)
# rotate view_point to form current view vector
rot_point = gimbal_dcm * view_point
# a line from the camera to the ground
line = Line(camera_point, rot_point)
# find the intersection with the ground
pt = line.plane_intersection(ground_plane, forward_only=True)
if pt is None:
# its pointing up into the sky
return None
(view_lat, view_lon) = mp_util.gps_offset(lat, lon, pt.y, pt.x)
icon = self.mpstate.map.icon('camera-small-red.png')
self.mpstate.map.add_object(
mp_slipmap.SlipIcon('gimbalview', (view_lat, view_lon),
icon,
layer='GimbalView',
rotation=0,
follow=False))