Spaces:
Sleeping
Sleeping
File size: 6,446 Bytes
8579cdc |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 |
"""GPS coordinate calculations using proper formulas."""
import math
from typing import Dict, List, Tuple
class DroneKnowledgeBase:
"""GPS coordinate calculations from drone-ai-knowledge-base.md."""
def __init__(self):
self.METERS_PER_DEGREE_LAT = 111000.0
self.direction_rules = {
'north': (1, 0), 'south': (-1, 0),
'east': (0, 1), 'west': (0, -1),
'northeast': (0.707, 0.707), 'northwest': (0.707, -0.707),
'southeast': (-0.707, 0.707), 'southwest': (-0.707, -0.707)
}
def calculate_coordinate_offset(self, lat: float, lon: float,
distance_m: float, direction: str) -> Tuple[float, float]:
"""Calculate new coordinates from offset."""
lat_deg_per_m = 1.0 / self.METERS_PER_DEGREE_LAT
# Avoid division by zero at poles and ensure minimal east/west offset
cos_lat = max(1e-6, abs(math.cos(math.radians(lat))))
lon_deg_per_m = 1.0 / (self.METERS_PER_DEGREE_LAT * cos_lat)
if direction.lower() not in self.direction_rules:
raise ValueError(f"Unknown direction: {direction}")
if direction.lower() == 'north':
return lat + distance_m * lat_deg_per_m, lon
elif direction.lower() == 'south':
return lat - distance_m * lat_deg_per_m, lon
elif direction.lower() == 'east':
return lat, lon + distance_m * lon_deg_per_m
elif direction.lower() == 'west':
return lat, lon - distance_m * lon_deg_per_m
else:
lat_factor, lon_factor = self.direction_rules[direction.lower()]
lat_offset = lat_factor * distance_m * lat_deg_per_m
lon_offset = lon_factor * distance_m * lon_deg_per_m
return lat + lat_offset, lon + lon_offset
def create_straight_line_waypoints(self, start_lat: float, start_lon: float,
direction: str, distance: float,
num_waypoints: int = 3) -> List[Dict]:
"""Create straight line waypoints."""
waypoints = []
segment_distance = distance / num_waypoints
for i in range(1, num_waypoints + 1):
current_distance = i * segment_distance
wp_lat, wp_lon = self.calculate_coordinate_offset(
start_lat, start_lon, current_distance, direction
)
waypoints.append({
'lat': wp_lat, 'lon': wp_lon, 'altitude': 70,
'hold_time': 0.0, 'accept_radius': 2.0
})
return waypoints
def create_survey_waypoints(self, center_lat: float, center_lon: float,
altitude: int, grid_size: int = 4) -> List[Dict]:
"""Create survey grid."""
waypoints = []
spacing = 200
for i in range(grid_size):
for j in range(grid_size):
north_offset = (i - grid_size/2) * spacing
east_offset = (j - grid_size/2) * spacing
wp_lat, _ = self.calculate_coordinate_offset(center_lat, center_lon, north_offset, 'north')
wp_lat, wp_lon = self.calculate_coordinate_offset(wp_lat, center_lon, east_offset, 'east')
waypoints.append({
'lat': wp_lat, 'lon': wp_lon, 'altitude': altitude,
'hold_time': 1.0, 'accept_radius': 3.0
})
return waypoints
def create_patrol_waypoints(self, center_lat: float, center_lon: float,
altitude: int, radius: float = 300) -> List[Dict]:
"""Create patrol perimeter with optimized coverage."""
waypoints = []
num_points = 8
for i in range(num_points):
angle = (2 * math.pi * i) / num_points
north_distance = radius * math.cos(angle)
east_distance = radius * math.sin(angle)
wp_lat, _ = self.calculate_coordinate_offset(center_lat, center_lon, north_distance, 'north')
wp_lat, wp_lon = self.calculate_coordinate_offset(wp_lat, center_lon, east_distance, 'east')
waypoints.append({
'lat': wp_lat, 'lon': wp_lon, 'altitude': altitude,
'hold_time': 2.0, 'accept_radius': 5.0
})
return waypoints
def create_photography_waypoints(self, center_lat: float, center_lon: float,
altitude: int) -> List[Dict]:
"""Create strategic photography positions with optimal angles."""
waypoints = []
# Enhanced photo positions with altitude variations
photo_offsets = [
(150, 150, 'northeast', 0),
(-150, 150, 'northwest', 10),
(-150, -150, 'southwest', 0),
(150, -150, 'southeast', 10),
(0, 0, 'center_overhead', 20)
]
for north_offset, east_offset, position, alt_adjustment in photo_offsets:
wp_lat, _ = self.calculate_coordinate_offset(center_lat, center_lon, north_offset, 'north')
wp_lat, wp_lon = self.calculate_coordinate_offset(wp_lat, center_lon, east_offset, 'east')
waypoints.append({
'lat': wp_lat, 'lon': wp_lon,
'altitude': altitude + alt_adjustment,
'hold_time': 3.0, 'accept_radius': 2.0
})
return waypoints
def create_inspection_waypoints(self, start_lat: float, start_lon: float,
direction: str, distance: float, altitude: int) -> List[Dict]:
"""Create detailed inspection waypoints with varying altitudes."""
waypoints = []
num_points = 6
for i in range(num_points):
segment_distance = (distance * (i + 1)) / num_points
alt_variation = (i - num_points//2) * 5 # Small altitude changes
wp_lat, wp_lon = self.calculate_coordinate_offset(
start_lat, start_lon, segment_distance, direction
)
waypoints.append({
'lat': wp_lat, 'lon': wp_lon,
'altitude': altitude + alt_variation,
'hold_time': 2.0, 'accept_radius': 1.5
})
return waypoints
|