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