233 lines
9.6 KiB
Python
233 lines
9.6 KiB
Python
import asyncio
|
|
import json
|
|
import math
|
|
import random
|
|
import websockets
|
|
|
|
FREQ_COLORS = {
|
|
170.0: "red", 170.1: "blue", 170.2: "green", 170.3: "orange", 170.4: "purple",
|
|
170.5: "brown", 170.6: "cyan", 170.7: "magenta", 170.8: "lime", 170.9: "teal",
|
|
171.0: "pink", 171.1: "navy", 171.2: "gold", 171.3: "salmon", 171.4: "olive",
|
|
171.5: "maroon", 171.6: "coral", 171.7: "indigo", 171.8: "violet", 171.9: "gray"
|
|
}
|
|
|
|
clients = set()
|
|
commands = asyncio.Queue()
|
|
|
|
center_lat, center_lon = -41.5917, 145.9344
|
|
|
|
current_lat, current_lon = center_lat, center_lon
|
|
|
|
active_orbit_target = None
|
|
orbit_path = []
|
|
orbit_index = 0
|
|
orbit_active = False
|
|
orbit_queue = []
|
|
completed_orbits = set()
|
|
active_freq = None
|
|
|
|
# Targets
|
|
targets = [
|
|
{"lat": -41.1568, "lon": 146.4257, "freq": 170.0},
|
|
{"lat": -41.3400, "lon": 146.2625, "freq": 170.1},
|
|
{"lat": -41.8190, "lon": 146.4390, "freq": 170.2},
|
|
{"lat": -42.0139, "lon": 145.6077, "freq": 170.3},
|
|
{"lat": -41.6302, "lon": 146.3656, "freq": 170.4},
|
|
{"lat": -41.6880, "lon": 146.1030, "freq": 170.5},
|
|
{"lat": -41.3899, "lon": 146.3310, "freq": 170.6},
|
|
{"lat": -41.4410, "lon": 145.5380, "freq": 170.7},
|
|
{"lat": -42.0270, "lon": 146.3340, "freq": 170.8},
|
|
{"lat": -41.4960, "lon": 146.4300, "freq": 170.9},
|
|
{"lat": -41.7510, "lon": 146.2030, "freq": 171.0},
|
|
{"lat": -41.8950, "lon": 146.4310, "freq": 171.1},
|
|
{"lat": -41.2390, "lon": 145.7600, "freq": 171.2},
|
|
{"lat": -41.3730, "lon": 145.5870, "freq": 171.3},
|
|
{"lat": -41.2900, "lon": 145.8420, "freq": 171.4},
|
|
{"lat": -41.8770, "lon": 145.7510, "freq": 171.5},
|
|
{"lat": -42.0130, "lon": 145.9500, "freq": 171.6},
|
|
{"lat": -41.5917, "lon": 145.9344, "freq": 171.7},
|
|
{"lat": -41.8000, "lon": 145.5000, "freq": 171.8},
|
|
{"lat": -41.5000, "lon": 146.0000, "freq": 171.9}
|
|
]
|
|
|
|
def haversine(lat1, lon1, lat2, lon2):
|
|
R = 6371
|
|
dlat = math.radians(lat2 - lat1)
|
|
dlon = math.radians(lon2 - lon1)
|
|
a = math.sin(dlat/2)**2 + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon/2)**2
|
|
return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|
|
|
|
def calculate_bearing(lat1, lon1, lat2, lon2):
|
|
phi1 = math.radians(lat1)
|
|
phi2 = math.radians(lat2)
|
|
delta_lon = math.radians(lon2 - lon1)
|
|
x = math.sin(delta_lon) * math.cos(phi2)
|
|
y = math.cos(phi1)*math.sin(phi2) - math.sin(phi1)*math.cos(phi2)*math.cos(delta_lon)
|
|
return (math.degrees(math.atan2(x, y)) + 360) % 360
|
|
|
|
def noisy_bearing(bearing, distance_km, min_noise=0.5, max_noise=10.0, max_effective_km=10.0):
|
|
distance_km = min(distance_km, max_effective_km)
|
|
noise = max_noise - (distance_km / max_effective_km) * (max_noise - min_noise)
|
|
return (bearing + random.uniform(-noise, noise)) % 360
|
|
|
|
def generate_converging_spiral(center_lat, center_lon, num_turns=10, points_per_turn=100, max_radius_km=50):
|
|
path = []
|
|
total_points = num_turns * points_per_turn
|
|
for i in range(total_points):
|
|
angle = 2 * math.pi * (i / points_per_turn)
|
|
radius_km = max_radius_km * (1 - i / total_points)
|
|
lat_offset = (radius_km * math.sin(angle)) / 111
|
|
lon_offset = (radius_km * math.cos(angle)) / (111 * math.cos(math.radians(center_lat)))
|
|
path.append((center_lat + lat_offset, center_lon + lon_offset))
|
|
return path
|
|
|
|
def generate_orbit(center_lat, center_lon, radius_km=0.1, num_points=100):
|
|
path = []
|
|
for i in range(num_points):
|
|
angle = 2 * math.pi * i / num_points
|
|
dlat = (radius_km * math.sin(angle)) / 111
|
|
dlon = (radius_km * math.cos(angle)) / (111 * math.cos(math.radians(center_lat)))
|
|
path.append((center_lat + dlat, center_lon + dlon))
|
|
return path
|
|
|
|
search_path = generate_converging_spiral(center_lat, center_lon, points_per_turn=1000)
|
|
|
|
async def simulate():
|
|
global orbit_active, orbit_path, orbit_index, current_lat, current_lon, active_orbit_target, active_freq, completed_orbits
|
|
|
|
point_index = 0
|
|
|
|
while True:
|
|
try:
|
|
while not commands.empty():
|
|
cmd = await commands.get()
|
|
if cmd.get("type") == "command" and cmd.get("action") == "orbit_target":
|
|
cmd_freq = cmd.get("freq")
|
|
if cmd_freq in completed_orbits:
|
|
print(f"🚫 Skipping orbit for {cmd_freq} — already completed.")
|
|
continue
|
|
|
|
if active_freq is None:
|
|
orbit_queue.append(cmd)
|
|
active_freq = cmd_freq
|
|
elif not orbit_active and cmd_freq == active_freq:
|
|
print(f"🔁 Updating active orbit target: {cmd}")
|
|
active_orbit_target["lat"] = cmd["lat"]
|
|
active_orbit_target["lon"] = cmd["lon"]
|
|
active_orbit_target["radius"] = cmd.get("radius", 0.1)
|
|
else:
|
|
print(f"⏳ Queuing orbit for later: freq={cmd_freq}")
|
|
orbit_queue.append(cmd)
|
|
|
|
except asyncio.QueueEmpty:
|
|
pass
|
|
|
|
step_km = haversine(*search_path[point_index % len(search_path)], *search_path[(point_index + 1) % len(search_path)])
|
|
|
|
if not orbit_active and active_orbit_target is None:
|
|
# Look for next valid orbit target
|
|
while orbit_queue:
|
|
next_cmd = orbit_queue.pop(0)
|
|
freq = next_cmd.get("freq")
|
|
if freq in completed_orbits:
|
|
print(f"🚫 Skipping orbit for {freq} — already completed.")
|
|
continue
|
|
print(f"🛰️ Starting new orbit for {freq}")
|
|
active_orbit_target = next_cmd
|
|
active_freq = freq
|
|
orbit_index = 0
|
|
orbit_path = []
|
|
break # found a valid one
|
|
|
|
|
|
|
|
if active_orbit_target:
|
|
orbit_center_lat = active_orbit_target["lat"]
|
|
orbit_center_lon = active_orbit_target["lon"]
|
|
radius = active_orbit_target.get("radius", 0.1)
|
|
dist = haversine(current_lat, current_lon, orbit_center_lat, orbit_center_lon)
|
|
|
|
if dist < 0.5:
|
|
if not orbit_active:
|
|
print(f"🛰️ Starting orbit around ({orbit_center_lat:.4f}, {orbit_center_lon:.4f})")
|
|
orbit_center_lat = active_orbit_target["lat"]
|
|
orbit_center_lon = active_orbit_target["lon"]
|
|
radius = active_orbit_target.get("radius", 0.1)
|
|
orbit_path = generate_orbit(orbit_center_lat, orbit_center_lon, radius_km=radius)
|
|
orbit_index = 0
|
|
orbit_active = True
|
|
last_sent_orbit = {"lat": orbit_center_lat, "lon": orbit_center_lon}
|
|
else:
|
|
bearing_to_target = calculate_bearing(current_lat, current_lon, orbit_center_lat, orbit_center_lon)
|
|
dlat = (step_km * math.cos(math.radians(bearing_to_target))) / 111
|
|
dlon = (step_km * math.sin(math.radians(bearing_to_target))) / (111 * math.cos(math.radians(current_lat)))
|
|
current_lat += dlat
|
|
current_lon += dlon
|
|
|
|
if orbit_active and orbit_path:
|
|
target_lat, target_lon = orbit_path[orbit_index % len(orbit_path)]
|
|
orbit_index += 1
|
|
current_lat = target_lat
|
|
current_lon = target_lon
|
|
|
|
if orbit_index >= len(orbit_path):
|
|
orbit_active = False
|
|
print(f"✅ Finished orbit for {active_freq}")
|
|
completed_orbits.add(active_freq)
|
|
active_freq = None
|
|
active_orbit_target = None
|
|
elif not active_orbit_target:
|
|
current_lat, current_lon = search_path[point_index % len(search_path)]
|
|
point_index += 1
|
|
|
|
await broadcast(json.dumps({"type": "drone", "lat": current_lat, "lon": current_lon, "alt": 100}))
|
|
|
|
for target in targets:
|
|
await asyncio.sleep(0.01)
|
|
distance_km = haversine(current_lat, current_lon, target["lat"], target["lon"])
|
|
if distance_km > 10:
|
|
continue
|
|
signal_strength = max(0.0, 1.0 - (distance_km / 10))
|
|
bearing = calculate_bearing(current_lat, current_lon, target["lat"], target["lon"])
|
|
noisy = noisy_bearing(bearing, distance_km)
|
|
mirror = (noisy + 180) % 360
|
|
await broadcast(json.dumps({
|
|
"type": "bearing",
|
|
"freq": target["freq"],
|
|
"lat": current_lat,
|
|
"lon": current_lon,
|
|
"bearing": noisy,
|
|
"mirror": mirror,
|
|
"alt": 100,
|
|
"weight": signal_strength,
|
|
"color": FREQ_COLORS[target["freq"]]
|
|
}))
|
|
|
|
async def websocket_handler(websocket):
|
|
clients.add(websocket)
|
|
print("🧪 Client connected")
|
|
try:
|
|
async for message in websocket:
|
|
print("📥 Received:", message)
|
|
try:
|
|
data = json.loads(message)
|
|
if data.get("type") == "command" and data.get("action") == "orbit_target":
|
|
print("🛰️ Queuing orbit command:", data)
|
|
await commands.put(data)
|
|
except Exception as e:
|
|
print("❌ Invalid message:", e)
|
|
finally:
|
|
clients.remove(websocket)
|
|
|
|
async def broadcast(message):
|
|
if clients:
|
|
await asyncio.gather(*(client.send(message) for client in clients))
|
|
|
|
async def main():
|
|
print("📡 Simulation server running at ws://localhost:8765")
|
|
server = websockets.serve(websocket_handler, "localhost", 8765)
|
|
await asyncio.gather(server, simulate())
|
|
|
|
if __name__ == "__main__":
|
|
asyncio.run(main())
|