SirBlob/simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Search and Flight Tracker Update

Browse Source
This commit is contained in:
SirBlobby
2026-02-20 16:11:10 -05:00
parent e2f805f3f3
commit e20c7e916d
4 changed files with 43 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
src/control/uav_controller.py
View File

@@ -10,8 +10,6 @@ from pymavlink import mavutil
import pymavlink.dialects.v20.ardupilotmega as mavlink
from pymavlink.dialects.v20.ardupilotmega import (
MAVLink_set_position_target_local_ned_message,
MAVLink_set_position_target_global_int_message,
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
MAV_FRAME_BODY_OFFSET_NED,
MAV_FRAME_LOCAL_NED,
MAV_CMD_DO_SET_MODE,
@@ -79,17 +77,6 @@ class Controller:
def update_state(self):
self._drain_messages()
def get_position(self) -> dict:
self.conn.mav.command_long_send(
self.conn.target_system,
self.conn.target_component,
mavlink.MAV_CMD_REQUEST_MESSAGE,
0, 33, 0, 0, 0, 0, 0, 0)
pos = self.conn.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
if pos is None:
return {'lat': 0, 'lon': 0}
return {'lat': pos.lat, 'lon': pos.lon}
def get_local_position(self) -> dict:
self.update_state()
return self.position.copy()
@@ -213,17 +200,6 @@ class Controller:
0, 89, 9, 0, 0, 0, 0, 0)
print("[UAV] Landing (LAND mode)")
def land_at(self, lat: int, lon: int):
self.conn.mav.command_long_send(
self.conn.target_system,
self.conn.target_component,
MAV_CMD_NAV_LAND,
0, 0, 0, 0, 0,
lat / 1e7 if abs(lat) > 1e6 else lat,
lon / 1e7 if abs(lon) > 1e6 else lon,
0)
print(f"[UAV] Landing at ({lat}, {lon})")
def move_pos_rel(self, x: float, y: float, z: float):
move_msg = MAVLink_set_position_target_local_ned_message(
int(perf_counter() * 1000),
@@ -264,16 +240,6 @@ class Controller:
0, 0, 0, 0, 0, 0, 0, 0)
self.conn.mav.send(move_msg)
def go_to(self, lat: int, lon: int, alt: float, yaw: float = 0):
move_msg = MAVLink_set_position_target_global_int_message(
int(perf_counter() * 1000),
self.conn.target_system,
self.conn.target_component,
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
504, lat, lon, alt,
0, 0, 0, 0, 0, 0, yaw, 0)
self.conn.mav.send(move_msg)
def distance(self, p1: Tuple, p2: Tuple) -> float:
return float(np.linalg.norm(np.asarray(p1) - np.asarray(p2)))
@@ -290,24 +256,6 @@ class Controller:
sleep(0.2)
return False
def wait_for_gps(self, timeout: float = 120.0):
print("[UAV] Waiting for GPS lock & EKF ...")
t0 = time.time()
while time.time() - t0 < timeout:
self._drain_messages()
msg = self.conn.recv_match(type='GPS_RAW_INT', blocking=True, timeout=2)
if msg and msg.fix_type >= 3:
print(f"\n[UAV] GPS fix: {msg.fix_type} sats: {msg.satellites_visible}")
print("[UAV] Waiting for EKF to settle ...")
# Wait briefly; let the arming loop handle the rest
for _ in range(2):
self._drain_messages()
sleep(1)
return True
elapsed = int(time.time() - t0)
print(f"\r[UAV] Waiting for GPS ... {elapsed}s ", end='', flush=True)
return False
def configure_speed_limits(self,
wpnav_speed=DEFAULT_WPNAV_SPEED,
wpnav_accel=DEFAULT_WPNAV_ACCEL,