diff --git a/aerpaw/client/uav_runner.py b/aerpaw/client/uav_runner.py index 2c83576..a9f530a 100644 --- a/aerpaw/client/uav_runner.py +++ b/aerpaw/client/uav_runner.py @@ -21,8 +21,11 @@ Binary Protocol: from enum import IntEnum from pathlib import Path import asyncio +import csv +import datetime import os import struct +import time import yaml from aerpawlib.runner import BasicRunner, entrypoint @@ -82,6 +85,53 @@ async def send_message_type(writer: asyncio.StreamWriter, msg_type: MessageType) await writer.drain() +def _gps_log_row(vehicle, line_num, writer): + """Sample vehicle state and write one CSV row (matches gps_logger.py format).""" + pos = vehicle.position + lat, lon = pos.lat, pos.lon + alt = round(float(str(pos.alt)), 3) + + batt = str(vehicle.battery) + volt = float(batt[16:batt.find(",")]) + + timestamp = datetime.datetime.now() + fix, num_sat = vehicle.gps.fix_type, vehicle.gps.satellites_visible + + if fix < 2: + lat, lon, alt = -999, -999, -999 + + vel = vehicle.velocity + attitude = vehicle.attitude + attitude_str = ( + "(" + ",".join(map(str, [attitude.pitch, attitude.yaw, attitude.roll])) + ")" + ) + + writer.writerow([line_num, lon, lat, alt, attitude_str, vel, volt, timestamp, fix, num_sat]) + + +async def _gps_log_loop(drone): + """Background async task that logs GPS data at 1Hz.""" + filename = f"/root/Results/GPS_DATA_{datetime.datetime.now().strftime('%Y-%m-%d_%H:%M:%S')}.csv" + print(f"[UAV] GPS logging to {filename}") + line_num = 1 + try: + with open(filename, "w+") as f: + writer = csv.writer(f) + while True: + if drone.connected: + _gps_log_row(drone, line_num, writer) + f.flush() + os.fsync(f) + line_num += 1 + else: + print("[UAV] [GPS] No vehicle heartbeat") + await asyncio.sleep(1.0) + except asyncio.CancelledError: + print(f"[UAV] GPS logging stopped ({line_num - 1} rows written)") + except Exception as e: + print(f"[UAV] GPS logging error: {e}") + + class UAVRunner(BasicRunner): def initialize_args(self, extra_args): """Load configuration from YAML config file.""" @@ -103,11 +153,6 @@ class UAVRunner(BasicRunner): @entrypoint async def run_mission(self, drone: Drone): """Main mission entry point.""" - # Enable built-in telemetry logging - drone._verbose_logging = True - drone._verbose_logging_file_prefix = "uav_telemetry" - drone._verbose_logging_delay = 1.0 # 1 Hz - print(f"[UAV] Connecting to controller at {self.server_ip}:{self.server_port}") # Retry connection up to 10 times (~30 seconds total) @@ -129,12 +174,16 @@ class UAVRunner(BasicRunner): print("[UAV] Failed to connect to controller after 10 attempts") return + log_task = None try: # Takeoff to above AERPAW minimum altitude print("[UAV] Taking off...") await drone.takeoff(25) print("[UAV] Takeoff complete, waiting for commands...") + # Start GPS logging in background + log_task = asyncio.create_task(_gps_log_loop(drone)) + # Command loop - handle TARGET, RTL, LAND, READY from controller waypoint_num = 0 while True: @@ -180,10 +229,7 @@ class UAVRunner(BasicRunner): print(f"[UAV] Sent ACK") print("[UAV] Landing...") await drone.land() - # Switch out of LAND mode so the drone is re-armable - from dronekit import VehicleMode - drone._vehicle.mode = VehicleMode("ALT_HOLD") - print("[UAV] Landed and disarmed (ALT_HOLD)") + print("[UAV] Landed and disarmed") await send_message_type(writer, MessageType.READY) print(f"[UAV] Sent READY") @@ -198,6 +244,9 @@ class UAVRunner(BasicRunner): print(f"[UAV] Error: {e}") finally: + if log_task is not None: + log_task.cancel() + await asyncio.gather(log_task, return_exceptions=True) writer.close() await writer.wait_closed() print("[UAV] Connection closed")