From 1a41e88243109f5111567a74e6a537c2127daedb Mon Sep 17 00:00:00 2001 From: Fawwaz Hameed Date: Sun, 28 Apr 2024 16:00:07 -0600 Subject: [PATCH 01/16] Update navigator.py Added send_land_message which sends precision landing to MAVlink Note: the class must be constructed with camera fov and camera resolution. --- src/modules/autopilot/navigator.py | 32 +++++++++++++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/src/modules/autopilot/navigator.py b/src/modules/autopilot/navigator.py index f0bc088..90a57aa 100644 --- a/src/modules/autopilot/navigator.py +++ b/src/modules/autopilot/navigator.py @@ -14,10 +14,14 @@ class Navigator: vehicle: dronekit.Vehicle = None - def __init__(self, vehicle, messenger_port): + def __init__(self, vehicle, messenger_port, camera_fov, camera_res): self.vehicle = vehicle self.mavlink_messenger = Messenger(messenger_port) + """Pass camera_fov and camera_res as tuple""" + self.camera_fov = camera_fov + self.camera_res = camera_res + def send_message(self, msg): self.__message(msg) @@ -274,3 +278,29 @@ def __condition_yaw(self, heading, relative=False): 0, 0, 0) # param 5 ~ 7 not used # send command to vehicle self.vehicle.send_mavlink(msg) + + + def send_land_message(x, y): + + """Sends a precision landing message to MAVlink + + :param x: The x offset from the landing zone + :param y: the y offset from the landing zone""" + + horizontal_fov = self.camera_fov[0] + vertical_fov = self.camera_fov[1] + + horizontal_resolution = self.camera_res[0] + vertical_resolution = self.camera_res[1] + + + msg = self.vehicle.message_factory.landing_target_encode( + 0, # time_boot_ms (not used) + 0, # target num + 0, # frame + (x-horizontal_resolution/2)*horizontal_fov/horizontal_resolution, + (y-vertical_resolution/2)*vertical_fov/vertical_resolution, + 0, # altitude. Not supported. + 0,0) # size of target in radians + self.vehicle.send_mavlink(msg) + self.vehicle.flush() \ No newline at end of file From dcb698e80bfe6b03a23cb5ac8119cf825a6821be Mon Sep 17 00:00:00 2001 From: Fawwaz Hameed Date: Thu, 16 May 2024 22:26:52 -0600 Subject: [PATCH 02/16] Altered LANDING_TARGET to conform to imaging format --- src/modules/autopilot/navigator.py | 38 +++++++++++++++--------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/modules/autopilot/navigator.py b/src/modules/autopilot/navigator.py index 90a57aa..cc449e4 100644 --- a/src/modules/autopilot/navigator.py +++ b/src/modules/autopilot/navigator.py @@ -14,13 +14,11 @@ class Navigator: vehicle: dronekit.Vehicle = None - def __init__(self, vehicle, messenger_port, camera_fov, camera_res): + def __init__(self, vehicle, messenger_port): self.vehicle = vehicle self.mavlink_messenger = Messenger(messenger_port) - """Pass camera_fov and camera_res as tuple""" - self.camera_fov = camera_fov - self.camera_res = camera_res + def send_message(self, msg): self.__message(msg) @@ -280,27 +278,29 @@ def __condition_yaw(self, heading, relative=False): self.vehicle.send_mavlink(msg) - def send_land_message(x, y): + def send_land_message(angle_x, angle_y, distance, size_x, size_y): """Sends a precision landing message to MAVlink - :param x: The x offset from the landing zone - :param y: the y offset from the landing zone""" - - horizontal_fov = self.camera_fov[0] - vertical_fov = self.camera_fov[1] + https://mavlink.io/en/messages/common.html#LANDING_TARGET + + https://mavlink.io/en/services/landing_target.html - horizontal_resolution = self.camera_res[0] - vertical_resolution = self.camera_res[1] + :param angle_x: The x angular offset of landing zone from the center of a downward facing camera [Units: radians] + :param angle_y: the y angular offset of the landing zone from the center of a downward facing camera [Units: radians] + :param distance: The distance between the drone and the landing zone (the OVERALL distance not the x or y distance) [Units: meters] + :param size_x: The size of the landing target with respect to the x axis. [Units: radians] + :param size_y: the size of the landing target with respect to the y axis [Units: radians]""" + msg = self.vehicle.message_factory.landing_target_encode( - 0, # time_boot_ms (not used) - 0, # target num - 0, # frame - (x-horizontal_resolution/2)*horizontal_fov/horizontal_resolution, - (y-vertical_resolution/2)*vertical_fov/vertical_resolution, - 0, # altitude. Not supported. - 0,0) # size of target in radians + angle_x, + angle_y, + distance, + size_x, + size_y + + ) self.vehicle.send_mavlink(msg) self.vehicle.flush() \ No newline at end of file From 2fb09e3cedd022791b2ef7da1be447641a6b5529 Mon Sep 17 00:00:00 2001 From: fadilm777 Date: Sat, 13 Sep 2025 10:56:10 -0600 Subject: [PATCH 03/16] changed analysis delegate structure and added beacon analysis delegate --- src/flight_tests/precision_landing.py | 14 +++ src/modules/autopilot/mavctl_advanced.py | 35 +++++++ src/modules/imaging/analysis.py | 113 ++++++++++++++--------- 3 files changed, 119 insertions(+), 43 deletions(-) create mode 100644 src/flight_tests/precision_landing.py diff --git a/src/flight_tests/precision_landing.py b/src/flight_tests/precision_landing.py new file mode 100644 index 0000000..99f1b6c --- /dev/null +++ b/src/flight_tests/precision_landing.py @@ -0,0 +1,14 @@ +from src.modules.imaging.analysis import AnalysisDelegate +from src.modules.mavctl.mavctl.connect import conn +from pymavlink import mavutil +from src.modules.mavctl.mavctl.messages.Navigator import Navigator +from src.modules.mavctl.mavctl.messages.messenger import Messenger +from src.modules.mavctl.mavctl.messages import advanced + +CONN_STR = "udp:127.0.0.1:14551" + +mav = conn.connect() +master = Navigator(mav) +master.send_status_message("MAVCTL: Online") + +imaging_analysis_delegate = AnalysisDelegate diff --git a/src/modules/autopilot/mavctl_advanced.py b/src/modules/autopilot/mavctl_advanced.py index e69de29..e4ad5a5 100644 --- a/src/modules/autopilot/mavctl_advanced.py +++ b/src/modules/autopilot/mavctl_advanced.py @@ -0,0 +1,35 @@ +from typing import Callable, Literal +from src.modules.mavctl.mavctl.messages import Navigator +from src.modules.imaging.analysis import AnalysisDelegate + +def do_precision_landing(master: Navigator, + analysis_subscriber: Callable, + mode: Literal["REQUIRED", "OPPORTUNISTIC"]) -> None: + """ + This function sets the drone into precision landing mode. + + Parameters: + analysis_subscriber: A function that accepts a callback function which updates + the landing target coordinates + + mode (str): Either "REQUIRED" or "OPPORTUNISTIC". + + REQUIRED: + The vehicle searches for a target if none is visible when + landing is initiated, and performs precision landing if found. + + OPPORTUNISTIC: + The vehicle uses precision landing only if the target is visible + at landing initiation; otherwise it performs a normal landing. + """ + def callback(coords): + altitude = master.get_altitude().terrain # Gets the altitude above the terrain (as seen by an altimeter) + target = Navigator.LandingTarget(x=coords[0], y=coords[1], z=altitude) + master.broadcast_landing_target(target) + + analysis_subscriber(callback) + + if mode == "OPPORTUNISTIC": + master.land(land_mode=1) + elif mode == "REQUIRED": + master.land(land_mode=2) diff --git a/src/modules/imaging/analysis.py b/src/modules/imaging/analysis.py index 0f50188..fdb5d33 100644 --- a/src/modules/imaging/analysis.py +++ b/src/modules/imaging/analysis.py @@ -1,4 +1,4 @@ -from typing import Callable, Optional, List, Callable, Any +from typing import Callable, Optional, List, Callable, Any, Tuple import threading # from multiprocessing import Process @@ -19,7 +19,7 @@ def __init__(self): self.resolution = (1920, 1080) -class Inference: +class ImagingInference: def __init__(self, bounding_box: BoundingBox, relative_alt): camera_attributes = CameraAttributes() position = bounding_box.position @@ -28,8 +28,53 @@ def __init__(self, bounding_box: BoundingBox, relative_alt): self.y = (position.y + size.y / 2) / camera_attributes.resolution[1] self.relative_alt = relative_alt +class AnalysisDelegate: -class ImageAnalysisDelegate: + def __init__(self) -> None: + self.thread: threading.Thread + self.loop = False + self.subscribers: List[Callable] = [] + + def _analyze_unit(self): + """Analyse and subscribe the result of analysis to subscribers""" + raise NotImplementedError + + def start(self): + """ + Will start the analysis process in another thread. + """ + self.loop = True + self.thread = threading.Thread(target=self._analysis_loop) + self.thread.start() + + def stop(self): + self.loop = False + assert self.thread is not None + self.thread.join() + + def _analysis_loop(self): + """ + Indefinitely run analysis. This should be run in another thread; + use `start()` to do so. + """ + while self.loop: + self._analyze_unit() + + def subscribe(self, callback: Callable): + """ + Subscribe to analysis updates. For example: + + def myhandler(image: Image.Image, bounding_box: BoundingBox): + if bounding_box is None: + print("No bounding box detected") + else: + print("Bounding box detected") + + imaging_process.subscribe(myhandler) + """ + self.subscribers.append(callback) + +class ImageAnalysisDelegate(AnalysisDelegate): """ Implements an imaging inference loops and provides several methods which can be used to query the latest image analysis results. @@ -40,15 +85,13 @@ class ImageAnalysisDelegate: Pass an `ImageAnalysisDebugger` when constructing to see a window with live results. - - TODO: geolocate the landing pad using the drone's location. """ def __init__(self, detector: LandingPadDetector, camera: CameraProvider, - location_provider: LocationProvider = None, - navigation_provider: Navigator = None, + location_provider: Optional[LocationProvider] = None, + navigation_provider: Optional[Navigator] = None, debugger: Optional[ImageAnalysisDebugger] = None): self.detector = detector self.camera = camera @@ -60,11 +103,11 @@ def __init__(self, self.location_provider = location_provider self.navigation_provider = navigation_provider - self.subscribers: List[Callable[[Image.Image, float, float], Any]] = [] self.camera_attributes = CameraAttributes() - self.thread = None - self.loop = True - def get_inference(self, bounding_box: BoundingBox) -> Inference: + + super().__init__() + + def get_inference(self, bounding_box: BoundingBox) -> ImagingInference: if self.location_provider is not None: altitude = self.location_provider.altitude() elif self.navigation_provider is not None: @@ -72,23 +115,11 @@ def get_inference(self, bounding_box: BoundingBox) -> Inference: else: raise ValueError("No altitude information provider available.") - inference = Inference(bounding_box, altitude) + inference = ImagingInference(bounding_box, altitude) return inference - def start(self): - """ - Will start the image analysis process in another thread. - """ - self.loop = True - self.thread = threading.Thread(target=self._analysis_loop) - # process = Process(target=self._analysis_loop) - self.thread.start() - # process.start() - # Use `threading` to start `self._analysis_loop` in another thread. - - def stop(self): - self.loop = False - self.thread.join() + def _analyze_unit(self): + self._analyze_image() def _analyze_image(self): """ @@ -114,24 +145,20 @@ def _analyze_image(self): else: subscriber(im, None) - def _analysis_loop(self): - """ - Indefinitely run image analysis. This should be run in another thread; - use `start()` to do so. - """ - while self.loop: - self._analyze_image() +class BeaconAnalysisDelegate(AnalysisDelegate): - def subscribe(self, callback: Callable): + def __init__(self, beaconCoordinate: Tuple[float, float]) -> None: + """ + Parameters: + beaconCoordinate: Coordinates of the beacon you wish to assign. format: (latitude, longitude) """ - Subscribe to image analysis updates. For example: + super().__init__() + self.beaconCoordinate = beaconCoordinate - def myhandler(image: Image.Image, bounding_box: BoundingBox): - if bounding_box is None: - print("No bounding box detected") - else: - print("Bounding box detected") + def _analyze_unit(self): + for subscriber in self.subscribers: + #TODO: Implement logic to calculate offset between drones current position and beacon coordinates + pass - imaging_process.subscribe(myhandler) - """ - self.subscribers.append(callback) + def set_beacon_coordinate(self, beaconCoordinate: Tuple[float, float]) -> None: + self.beaconCoordinate = beaconCoordinate From f84069ff758444abd135c2796cc2a30b861506ef Mon Sep 17 00:00:00 2001 From: fadilm777 Date: Sat, 13 Sep 2025 10:56:51 -0600 Subject: [PATCH 04/16] update mavctl commit hash --- src/modules/mavctl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavctl b/src/modules/mavctl index 4f757ed..6d7af2b 160000 --- a/src/modules/mavctl +++ b/src/modules/mavctl @@ -1 +1 @@ -Subproject commit 4f757edc3df38e3cf6ca60eb8e047b508a9ba369 +Subproject commit 6d7af2b395043098df27eaa29050396c983be33b From 902156ea4a2aefb1e902deb08b8183d8c6e23a73 Mon Sep 17 00:00:00 2001 From: fadilm777 Date: Sat, 13 Sep 2025 13:43:36 -0600 Subject: [PATCH 05/16] more structure --- src/modules/autopilot/mavctl_advanced.py | 20 ++++++----- src/modules/imaging/analysis.py | 44 +++++++++++++++++++----- src/modules/mavctl | 2 +- 3 files changed, 48 insertions(+), 18 deletions(-) diff --git a/src/modules/autopilot/mavctl_advanced.py b/src/modules/autopilot/mavctl_advanced.py index e4ad5a5..41ab2c6 100644 --- a/src/modules/autopilot/mavctl_advanced.py +++ b/src/modules/autopilot/mavctl_advanced.py @@ -1,9 +1,9 @@ from typing import Callable, Literal -from src.modules.mavctl.mavctl.messages import Navigator -from src.modules.imaging.analysis import AnalysisDelegate +from src.modules.mavctl.mavctl.messages.navigator import Navigator, LandingTarget +from src.modules.imaging.analysis import AnalysisDelegate, AnalysisResult def do_precision_landing(master: Navigator, - analysis_subscriber: Callable, + analysisDelegate: AnalysisDelegate, mode: Literal["REQUIRED", "OPPORTUNISTIC"]) -> None: """ This function sets the drone into precision landing mode. @@ -22,12 +22,16 @@ def do_precision_landing(master: Navigator, The vehicle uses precision landing only if the target is visible at landing initiation; otherwise it performs a normal landing. """ - def callback(coords): - altitude = master.get_altitude().terrain # Gets the altitude above the terrain (as seen by an altimeter) - target = Navigator.LandingTarget(x=coords[0], y=coords[1], z=altitude) - master.broadcast_landing_target(target) + def callback(position: AnalysisResult): + if position: + altitude = master.get_altitude().terrain # Gets the altitude above the terrain (as seen by an altimeter) + target = LandingTarget(right=position.right, + forward=position.front, + altitude=altitude) - analysis_subscriber(callback) + master.broadcast_landing_target(target) + + analysisDelegate.subscribe(callback) if mode == "OPPORTUNISTIC": master.land(land_mode=1) diff --git a/src/modules/imaging/analysis.py b/src/modules/imaging/analysis.py index fdb5d33..1a043e6 100644 --- a/src/modules/imaging/analysis.py +++ b/src/modules/imaging/analysis.py @@ -1,3 +1,5 @@ +from abc import abstractmethod +from dataclasses import dataclass from typing import Callable, Optional, List, Callable, Any, Tuple import threading @@ -28,13 +30,33 @@ def __init__(self, bounding_box: BoundingBox, relative_alt): self.y = (position.y + size.y / 2) / camera_attributes.resolution[1] self.relative_alt = relative_alt +@dataclass +class AnalysisResult: + """ + Represents the result of an analysis performed by an `AnalysisDelegate`. + + This data class stores the relative position of the detected target + with respect to the drone's current location and orientation. + + Attributes: + front (float): The forward offset of the target in meters. + Positive values indicate the target is ahead of the drone, + negative values indicate it is behind. + left (float): The lateral offset of the target in meters. + Positive values indicate the target is to the right of the drone, + negative values indicate it is to the left. + """ + front: float + right: float + class AnalysisDelegate: def __init__(self) -> None: self.thread: threading.Thread self.loop = False - self.subscribers: List[Callable] = [] + self.subscribers: List[Callable[[AnalysisResult], None]] = [] + @abstractmethod def _analyze_unit(self): """Analyse and subscribe the result of analysis to subscribers""" raise NotImplementedError @@ -43,9 +65,10 @@ def start(self): """ Will start the analysis process in another thread. """ - self.loop = True - self.thread = threading.Thread(target=self._analysis_loop) - self.thread.start() + if self.loop is False: + self.loop = True + self.thread = threading.Thread(target=self._analysis_loop) + self.thread.start() def stop(self): self.loop = False @@ -58,9 +81,13 @@ def _analysis_loop(self): use `start()` to do so. """ while self.loop: - self._analyze_unit() + try: + self._analyze_unit() + except Exception as e: + print("Error in analysis loop: ", e) + self.loop = False - def subscribe(self, callback: Callable): + def subscribe(self, callback: Callable[[AnalysisResult], None]): """ Subscribe to analysis updates. For example: @@ -141,9 +168,8 @@ def _analyze_image(self): if inference: x, y = get_object_location(self.camera_attributes, inference) - subscriber(im, (x, y)) - else: - subscriber(im, None) + analysis_result = AnalysisResult(front=y, right=x) + subscriber(analysis_result) class BeaconAnalysisDelegate(AnalysisDelegate): diff --git a/src/modules/mavctl b/src/modules/mavctl index 6d7af2b..3ebf782 160000 --- a/src/modules/mavctl +++ b/src/modules/mavctl @@ -1 +1 @@ -Subproject commit 6d7af2b395043098df27eaa29050396c983be33b +Subproject commit 3ebf782aaf560bf629d26c31ec18f02bdd72b0a5 From 2e383da09220d5d8c70dd319a60322f58b5ac946 Mon Sep 17 00:00:00 2001 From: fadilm777 Date: Sat, 13 Sep 2025 13:56:59 -0600 Subject: [PATCH 06/16] made subscription to delegates more general and inheritable --- src/modules/imaging/analysis.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/modules/imaging/analysis.py b/src/modules/imaging/analysis.py index 1a043e6..7da98a7 100644 --- a/src/modules/imaging/analysis.py +++ b/src/modules/imaging/analysis.py @@ -87,6 +87,11 @@ def _analysis_loop(self): print("Error in analysis loop: ", e) self.loop = False + def subscriberService(self, analysis_result: AnalysisResult): + """Service function that subscribes the reult to every subscriber""" + for subscriber in self.subscribers: + subscriber(analysis_result) + def subscribe(self, callback: Callable[[AnalysisResult], None]): """ Subscribe to analysis updates. For example: @@ -162,14 +167,13 @@ def _analyze_image(self): if bounding_box is not None: self.debugger.set_bounding_box(bounding_box) - for subscriber in self.subscribers: - if bounding_box: - inference = self.get_inference(bounding_box) - if inference: - x, y = get_object_location(self.camera_attributes, - inference) - analysis_result = AnalysisResult(front=y, right=x) - subscriber(analysis_result) + if bounding_box: + inference = self.get_inference(bounding_box) + if inference: + x, y = get_object_location(self.camera_attributes, + inference) + analysis_result = AnalysisResult(front=y, right=x) + self.subscriberService(analysis_result) class BeaconAnalysisDelegate(AnalysisDelegate): From b33287e10e8c540fb8bfb03ea2fb3190eabe5d9e Mon Sep 17 00:00:00 2001 From: Fawwaz Hameed Date: Sat, 13 Sep 2025 20:45:31 -0600 Subject: [PATCH 07/16] still broken :(((((( --- src/flight_tests/precision_landing.py | 26 ++++++++++++++----- src/modules/autopilot/mavctl_advanced.py | 4 +++ .../georeference/inference_georeference.py | 11 ++++++++ src/modules/imaging/analysis.py | 26 ++++++++++--------- 4 files changed, 49 insertions(+), 18 deletions(-) diff --git a/src/flight_tests/precision_landing.py b/src/flight_tests/precision_landing.py index 99f1b6c..42ade09 100644 --- a/src/flight_tests/precision_landing.py +++ b/src/flight_tests/precision_landing.py @@ -1,14 +1,28 @@ -from src.modules.imaging.analysis import AnalysisDelegate +from src.modules.imaging.analysis import BeaconAnalysisDelegate from src.modules.mavctl.mavctl.connect import conn -from pymavlink import mavutil -from src.modules.mavctl.mavctl.messages.Navigator import Navigator +from src.modules.mavctl.mavctl.messages.navigator import Navigator from src.modules.mavctl.mavctl.messages.messenger import Messenger from src.modules.mavctl.mavctl.messages import advanced +from src.modules.mavctl.mavctl.messages.location import LocationGlobal +import src.modules.autopilot.mavctl_advanced as mavctl_advanced +import time CONN_STR = "udp:127.0.0.1:14551" +LOCATION = LocationGlobal(lat = 53.4954, lon = -113.5512) -mav = conn.connect() +mav = conn.connect(CONN_STR) master = Navigator(mav) -master.send_status_message("MAVCTL: Online") +messenger = Messenger(14553) +#Start the debug imaging analsis delegate +#Broadcasts a fixed lat and lon to the debug imaging analysis delegate for testing purposes for precision landing +beacon_delegate = BeaconAnalysisDelegate(LOCATION, navigator=master) +beacon_delegate.start() +messenger.send("MAVCTL: Online") -imaging_analysis_delegate = AnalysisDelegate +while master.set_mode_wait() and master.wait_vehicle_armed(): + pass + +master.takeoff(10) +time.sleep(5) +print("Takeoff complete after sleeping 5 seconds") +mavctl_advanced.do_precision_landing(master, beacon_delegate, mode="REQUIRED") diff --git a/src/modules/autopilot/mavctl_advanced.py b/src/modules/autopilot/mavctl_advanced.py index 41ab2c6..ed8fc54 100644 --- a/src/modules/autopilot/mavctl_advanced.py +++ b/src/modules/autopilot/mavctl_advanced.py @@ -5,6 +5,7 @@ def do_precision_landing(master: Navigator, analysisDelegate: AnalysisDelegate, mode: Literal["REQUIRED", "OPPORTUNISTIC"]) -> None: + print("do_precision_landing start point") """ This function sets the drone into precision landing mode. @@ -30,8 +31,11 @@ def callback(position: AnalysisResult): altitude=altitude) master.broadcast_landing_target(target) + print("after the callback func def") analysisDelegate.subscribe(callback) + + print("subscription occurred") if mode == "OPPORTUNISTIC": master.land(land_mode=1) diff --git a/src/modules/georeference/inference_georeference.py b/src/modules/georeference/inference_georeference.py index 385f3fb..4ae3c2d 100644 --- a/src/modules/georeference/inference_georeference.py +++ b/src/modules/georeference/inference_georeference.py @@ -10,6 +10,7 @@ where X, Y are in meters """ # TODO: Requires a circular-import... but we only need these for type annotations +from dataclasses import dataclass import numpy as np from math import cos, tan, atan, radians, degrees, sqrt import pyproj @@ -21,6 +22,16 @@ if TYPE_CHECKING: from ..imaging.analysis import CameraAttributes, Inference +@dataclass +class Coordinate: + ''' + Data Class for storing global coordinates + + ''' + + lat: float + lon: float + def LonLat_To_XY(lon, lat, zone=12): """ diff --git a/src/modules/imaging/analysis.py b/src/modules/imaging/analysis.py index 7da98a7..a725a9d 100644 --- a/src/modules/imaging/analysis.py +++ b/src/modules/imaging/analysis.py @@ -3,16 +3,17 @@ from typing import Callable, Optional, List, Callable, Any, Tuple import threading + +from pymavlink.mavutil import location # from multiprocessing import Process from dep.labeller.benchmarks.detector import LandingPadDetector, BoundingBox +from src.modules.mavctl.mavctl.messages.location import LocationGlobal from .camera import CameraProvider from .debug import ImageAnalysisDebugger from ..georeference.inference_georeference import get_object_location from .location import LocationProvider -from ..autopilot.navigator import Navigator from PIL import Image -import os - +from src.modules.mavctl.mavctl.messages.navigator import Navigator class CameraAttributes: def __init__(self): @@ -81,11 +82,8 @@ def _analysis_loop(self): use `start()` to do so. """ while self.loop: - try: - self._analyze_unit() - except Exception as e: - print("Error in analysis loop: ", e) - self.loop = False + print("loop started") + self._analyze_unit() def subscriberService(self, analysis_result: AnalysisResult): """Service function that subscribes the reult to every subscriber""" @@ -177,18 +175,22 @@ def _analyze_image(self): class BeaconAnalysisDelegate(AnalysisDelegate): - def __init__(self, beaconCoordinate: Tuple[float, float]) -> None: + def __init__(self, beaconCoordinate: LocationGlobal, navigator: Navigator) -> None: """ Parameters: beaconCoordinate: Coordinates of the beacon you wish to assign. format: (latitude, longitude) """ + print("beacon has been inited") super().__init__() self.beaconCoordinate = beaconCoordinate + self.navigator = navigator def _analyze_unit(self): - for subscriber in self.subscribers: - #TODO: Implement logic to calculate offset between drones current position and beacon coordinates - pass + geovector = self.navigator.get_geovector(self.beaconCoordinate) + print(geovector) + result = AnalysisResult(front=geovector[1], right=geovector[0]) + self.subscriberService(result) + pass def set_beacon_coordinate(self, beaconCoordinate: Tuple[float, float]) -> None: self.beaconCoordinate = beaconCoordinate From e5efd0034760c78610544eacbcd8836a20f5bf0e Mon Sep 17 00:00:00 2001 From: Fawwaz Hameed Date: Sun, 14 Sep 2025 11:48:17 -0600 Subject: [PATCH 08/16] no precision landing test for today, leaving it for next time --- src/flight_tests/demo_flight.py | 25 +++++++++++++++++++++++++ src/flight_tests/precision_landing.py | 15 ++++++++------- src/modules/mavctl | 2 +- 3 files changed, 34 insertions(+), 8 deletions(-) create mode 100644 src/flight_tests/demo_flight.py diff --git a/src/flight_tests/demo_flight.py b/src/flight_tests/demo_flight.py new file mode 100644 index 0000000..90fa25e --- /dev/null +++ b/src/flight_tests/demo_flight.py @@ -0,0 +1,25 @@ +from src.modules.imaging.analysis import BeaconAnalysisDelegate +from src.modules.mavctl.mavctl.connect import conn +from src.modules.mavctl.mavctl.messages.navigator import Navigator, LandingTarget +from src.modules.mavctl.mavctl.messages.messenger import Messenger +from src.modules.mavctl.mavctl.messages import advanced +from src.modules.mavctl.mavctl.messages.location import LocationGlobal +import src.modules.autopilot.mavctl_advanced as mavctl_advanced +import time + +CONN_STR = "udp:127.0.0.1:14551" +mav = conn.connect(CONN_STR) +master = Navigator(mav) +messenger = Messenger(14553) +messenger.send("MAVCTL: Online") + +while master.set_mode_wait() and master.wait_vehicle_armed(): + pass + +master.takeoff(10) +time.sleep(5) +advanced.simple_goto_local(master, 50, 50, 20) +time.sleep(20) +advanced.simple_goto_local(master, 50, -50, 20) +time.sleep(10) +master.return_to_launch() diff --git a/src/flight_tests/precision_landing.py b/src/flight_tests/precision_landing.py index 42ade09..e4d920f 100644 --- a/src/flight_tests/precision_landing.py +++ b/src/flight_tests/precision_landing.py @@ -1,6 +1,6 @@ from src.modules.imaging.analysis import BeaconAnalysisDelegate from src.modules.mavctl.mavctl.connect import conn -from src.modules.mavctl.mavctl.messages.navigator import Navigator +from src.modules.mavctl.mavctl.messages.navigator import Navigator, LandingTarget from src.modules.mavctl.mavctl.messages.messenger import Messenger from src.modules.mavctl.mavctl.messages import advanced from src.modules.mavctl.mavctl.messages.location import LocationGlobal @@ -8,15 +8,12 @@ import time CONN_STR = "udp:127.0.0.1:14551" -LOCATION = LocationGlobal(lat = 53.4954, lon = -113.5512) +landing_target = LandingTarget(0.52, 0.52, 10) +print(landing_target) mav = conn.connect(CONN_STR) master = Navigator(mav) messenger = Messenger(14553) -#Start the debug imaging analsis delegate -#Broadcasts a fixed lat and lon to the debug imaging analysis delegate for testing purposes for precision landing -beacon_delegate = BeaconAnalysisDelegate(LOCATION, navigator=master) -beacon_delegate.start() messenger.send("MAVCTL: Online") while master.set_mode_wait() and master.wait_vehicle_armed(): @@ -25,4 +22,8 @@ master.takeoff(10) time.sleep(5) print("Takeoff complete after sleeping 5 seconds") -mavctl_advanced.do_precision_landing(master, beacon_delegate, mode="REQUIRED") +master.broadcast_landing_target(landing_target=landing_target) +msg = master.mav.recv_match(type="COMMAND_ACK", blocking=True) +print(msg) +print("Landing Target Broadcasted") +time.sleep(2) diff --git a/src/modules/mavctl b/src/modules/mavctl index 3ebf782..eb02a4e 160000 --- a/src/modules/mavctl +++ b/src/modules/mavctl @@ -1 +1 @@ -Subproject commit 3ebf782aaf560bf629d26c31ec18f02bdd72b0a5 +Subproject commit eb02a4ea4fc2d2a12db435d881efd1afb6ab9d3d From a602a9da5cfa1745326b9c44be51bdc75ef0db7c Mon Sep 17 00:00:00 2001 From: Fawwaz Hameed Date: Sun, 14 Sep 2025 12:00:59 -0600 Subject: [PATCH 09/16] mavctl commits --- src/modules/mavctl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavctl b/src/modules/mavctl index eb02a4e..e644980 160000 --- a/src/modules/mavctl +++ b/src/modules/mavctl @@ -1 +1 @@ -Subproject commit eb02a4ea4fc2d2a12db435d881efd1afb6ab9d3d +Subproject commit e644980364cec47add18f7f0a3d6f53e2b64b3b2 From 3ba01be3d179c711e0597faeb624504b87740a8f Mon Sep 17 00:00:00 2001 From: Fawwaz Hameed Date: Tue, 23 Sep 2025 21:20:03 -0600 Subject: [PATCH 10/16] refactoring --- src/flight_tests/precision_land_test_FH.py | 113 --------------------- src/flight_tests/precision_landing.py | 13 ++- 2 files changed, 6 insertions(+), 120 deletions(-) delete mode 100644 src/flight_tests/precision_land_test_FH.py diff --git a/src/flight_tests/precision_land_test_FH.py b/src/flight_tests/precision_land_test_FH.py deleted file mode 100644 index b03bc05..0000000 --- a/src/flight_tests/precision_land_test_FH.py +++ /dev/null @@ -1,113 +0,0 @@ -''' -Idea behind this flight test script is to fly to a waypoint and test the built-in precision landing component. - -Eventually this script will be adapted to support the ability to send rangefinder data to the pixhawk so that more accurate precision landing is possible. - -''' - -import time -import threading - -from dronekit import connect, VehicleMode, LocationGlobal - -from src.modules.autopilot import navigator -from src.modules.autopilot import lander -import src.modules.autopilot.altimeter as altimeter - -from src.modules.imaging import analysis -from src.modules.imaging import camera -from src.modules.imaging import debug -from src.modules.imaging import location -from dep.labeller.benchmarks import yolo - -mavlink_delegate = location.MAVLinkDelegate() -location_provider = location.MAVLinkLocationProvider(mavlink_delegate) -location_provider = location.DebugLocationProvider() -location_provider.debug_change_location(altitude=1) - -detector = yolo.YoloDetector(weights="landing_nano.pt") -cam = camera.RPiCamera() -debugger = debug.ImageAnalysisDebugger() -debugger = None -img_analysis = analysis.ImageAnalysisDelegate(detector, cam, location_provider, - debugger) -mavlink_delegate.run() - -CONN_STR = "udp:127.0.0.1:14551" -MESSENGER_PORT = 14552 - -drone = connect(CONN_STR, wait_ready=False) - -nav = navigator.Navigator(drone, MESSENGER_PORT) -lander = lander.Lander() -altimeter = altimeter.XM125(average_window=5) -altimeter.begin() - - -nav.send_status_message("Shepard is online") - -while not (drone.armed and drone.mode == VehicleMode("GUIDED")): - pass - -nav.send_status_message("Executing mission") -time.sleep(2) - -nav.takeoff(10) -start_coords = drone.location.global_relative_frame -time.sleep(2) - -MAX_GROUND_SPEED = 20 -TIME = 5 * 60 # 5 minutes -ALTITUDE = 15 -precision_land_enable = False - - -img_analysis.subscribe(lambda _, x, y: precision_land(x, y)) -img_analysis.start() - - - -waypoint = [10, 10, 20] - - -# MODIFY THIS AS REQUIRED -speed = nav.optimum_speed(TIME, [waypoint]) - -#checking to ensure ground speed is safe -assert speed > 0 -assert speed < MAX_GROUND_SPEED - -#drone.groundspeed = speed -#nav.send_status_message(f"Ground speed set to {speed} m/s") - -# workaround to get the speed to set properly for the actual waypoints -nav.set_position_relative(0, 0) - -time.sleep(1) -nav.set_speed(speed) -time.sleep(1) - - -nav.send_status_message(f"Moving to pre-set waypoints") -nav.set_altitude_position(waypoint[0], - waypoint[1], - waypoint[2], - battery=None, - hard_cutoff_enable=False) - -precision_land_enable = True - -type_mask = nav.generate_typemask([0, 1, 2]) - -current_alt = drone.location.global_relative_frame.alt - -def precision_land(x, y): - - # NEEDS TO BE MODIFIED TO ACTUALLY GO TO COORDINATES RELEVATIVE TO THE ORIGIN OF THIS REFERENCE FRAME - current_alt = drone.location.global_relative_frame.alt - if precision_land_enable and current_alt >= 0.5: - nav.set_position_target_local_ned(x = x, y = y, z = 0, type_mask=type_mask) - elif current_alt < 0.5: - nav.land() - precision_land_enable = False - diff --git a/src/flight_tests/precision_landing.py b/src/flight_tests/precision_landing.py index e4d920f..6df56a3 100644 --- a/src/flight_tests/precision_landing.py +++ b/src/flight_tests/precision_landing.py @@ -8,7 +8,7 @@ import time CONN_STR = "udp:127.0.0.1:14551" -landing_target = LandingTarget(0.52, 0.52, 10) +landing_target = LandingTarget(0.52, 0.52, 0) print(landing_target) mav = conn.connect(CONN_STR) @@ -18,12 +18,11 @@ while master.set_mode_wait() and master.wait_vehicle_armed(): pass - master.takeoff(10) time.sleep(5) print("Takeoff complete after sleeping 5 seconds") -master.broadcast_landing_target(landing_target=landing_target) -msg = master.mav.recv_match(type="COMMAND_ACK", blocking=True) -print(msg) -print("Landing Target Broadcasted") -time.sleep(2) +master.land(2) +while True: + master.broadcast_landing_target(landing_target=landing_target) + print("Landing Target Broadcasted") + time.sleep(1) From c9a44288965394b8cb3adb31326358743ff2cf92 Mon Sep 17 00:00:00 2001 From: UA-24 Raspi 4 Date: Sat, 25 Oct 2025 20:06:00 +0100 Subject: [PATCH 11/16] flight test stuff --- bin/run-nav.sh | 3 +-- install/shepard.service | 4 ++-- src/flight_tests/mavctl-test.py | 23 +++++++++++++++++++++++ src/flight_tests/vel_control_test.py | 2 +- 4 files changed, 27 insertions(+), 5 deletions(-) create mode 100644 src/flight_tests/mavctl-test.py diff --git a/bin/run-nav.sh b/bin/run-nav.sh index 28b5e89..f83160b 100755 --- a/bin/run-nav.sh +++ b/bin/run-nav.sh @@ -6,5 +6,4 @@ imgfile="log/$(date +'%x-%X').imglog" mkdir -p "$(dirname "$logfile")" mkdir -p "$(dirname "$imgfile")" -PYTHONUNBUFFERED=1 PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" python3 src/aeac2025/task2.py 2>&1 | tee "$logfile" & -PYTHONUNBUFFERED=1 PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" python3 src/video_server/server.py 2>&1 | tee "$imgfile" +PYTHONUNBUFFERED=1 PYTHONPATH=".:dep/labeller:dep/labeller/third_party/yolov5" python3 src/flight_tests/mavctl-test.py 2>&1 | tee "$logfile" diff --git a/install/shepard.service b/install/shepard.service index 8a1e835..39b640a 100644 --- a/install/shepard.service +++ b/install/shepard.service @@ -3,8 +3,8 @@ Description=Shepard After=login.target [Service] -ExecStart=/home/uaarg/Documents/shepard/bin/run-nav.sh -WorkingDirectory=/home/uaarg/Documents/shepard +ExecStart=/home/uaarg/shepard/bin/run-nav.sh +WorkingDirectory=/home/uaarg/shepard [Install] WantedBy=multi-user.target diff --git a/src/flight_tests/mavctl-test.py b/src/flight_tests/mavctl-test.py new file mode 100644 index 0000000..bc93b82 --- /dev/null +++ b/src/flight_tests/mavctl-test.py @@ -0,0 +1,23 @@ +from src.modules.mavctl.connect.conn import Connect +from pymavlink import mavutil +from src.modules.mavctl.messages.navigator import Navigator +from src.modules.mavctl.messages.messenger import Messenger +from src.modules.mavctl.messages import advanced +import time + +CONN_STR = "tcp:127.0.0.1:14550" + +mav = Connect(ip = CONN_STR) +master = Navigator(mav.mav) + +while master.wait_vehicle_armed(): + pass + +while not master.set_mode_wait(): + pass + +master.takeoff(10) +time.sleep(5) +advanced.simple_goto_global(master, 53.496970, -113.545194, 20) + +master.return_to_launch() diff --git a/src/flight_tests/vel_control_test.py b/src/flight_tests/vel_control_test.py index fffd1cf..9750aac 100644 --- a/src/flight_tests/vel_control_test.py +++ b/src/flight_tests/vel_control_test.py @@ -8,7 +8,7 @@ from src.modules.autopilot import navigator from src.modules.autopilot import lander -CONN_STR = "udp:127.0.0.1:14551" +CONN_STR = "tcp:127.0.0.1:14550" MESSENGER_PORT = 14552 drone = connect(CONN_STR, wait_ready=False) From 7335018fe2f3247157465f1477eb0dfa64b532ed Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Tue, 4 Nov 2025 20:03:44 -0700 Subject: [PATCH 12/16] It works in Gazebogit statusgit status! --- src/flight_tests/precision_landing.py | 15 +++++++-------- src/modules/mavctl | 2 +- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/flight_tests/precision_landing.py b/src/flight_tests/precision_landing.py index 6df56a3..4225772 100644 --- a/src/flight_tests/precision_landing.py +++ b/src/flight_tests/precision_landing.py @@ -1,20 +1,19 @@ from src.modules.imaging.analysis import BeaconAnalysisDelegate -from src.modules.mavctl.mavctl.connect import conn from src.modules.mavctl.mavctl.messages.navigator import Navigator, LandingTarget +from src.modules.mavctl.mavctl.connect.conn import Connect from src.modules.mavctl.mavctl.messages.messenger import Messenger from src.modules.mavctl.mavctl.messages import advanced from src.modules.mavctl.mavctl.messages.location import LocationGlobal import src.modules.autopilot.mavctl_advanced as mavctl_advanced import time -CONN_STR = "udp:127.0.0.1:14551" -landing_target = LandingTarget(0.52, 0.52, 0) +CONN_STR = "udp:127.0.0.1:14550" +landing_target = LandingTarget(0.5, 0.5, 5) print(landing_target) -mav = conn.connect(CONN_STR) -master = Navigator(mav) -messenger = Messenger(14553) -messenger.send("MAVCTL: Online") +connect = Connect(ip=CONN_STR) +master = Navigator(connect.mav) + while master.set_mode_wait() and master.wait_vehicle_armed(): pass @@ -25,4 +24,4 @@ while True: master.broadcast_landing_target(landing_target=landing_target) print("Landing Target Broadcasted") - time.sleep(1) + time.sleep(0.5) diff --git a/src/modules/mavctl b/src/modules/mavctl index 6695f0a..3dd927c 160000 --- a/src/modules/mavctl +++ b/src/modules/mavctl @@ -1 +1 @@ -Subproject commit 6695f0a04ee79e1a4999e826820ec0007d4724ec +Subproject commit 3dd927c23725e3421f545b3b70920e36cb0e0994 From 29db3686a7aad316c1f1793a2015bcecd0968ff2 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Tue, 4 Nov 2025 20:11:27 -0700 Subject: [PATCH 13/16] modified to include beacons for precision landing testing --- src/flight_tests/precision_landing.py | 7 +++++-- src/modules/mavctl | 2 +- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/flight_tests/precision_landing.py b/src/flight_tests/precision_landing.py index 4225772..1141e5c 100644 --- a/src/flight_tests/precision_landing.py +++ b/src/flight_tests/precision_landing.py @@ -8,12 +8,15 @@ import time CONN_STR = "udp:127.0.0.1:14550" -landing_target = LandingTarget(0.5, 0.5, 5) -print(landing_target) +beaconCoordinate = LocationGlobal(53.495534, -113.538215, 10) + connect = Connect(ip=CONN_STR) master = Navigator(connect.mav) +beacon = BeaconAnalysisDelegate(beaconCoordinate=beaconCoordinate, navigator=master) + + while master.set_mode_wait() and master.wait_vehicle_armed(): pass diff --git a/src/modules/mavctl b/src/modules/mavctl index 3dd927c..d31619f 160000 --- a/src/modules/mavctl +++ b/src/modules/mavctl @@ -1 +1 @@ -Subproject commit 3dd927c23725e3421f545b3b70920e36cb0e0994 +Subproject commit d31619f66336623c91078c75abd0489138e670e5 From a7ac9ae4471ff3afaae6c28a19120f3d1f47a3a3 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 5 Dec 2025 17:40:23 -0700 Subject: [PATCH 14/16] Created Aruco marker detection --- src/modules/imaging/detector.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/modules/imaging/detector.py b/src/modules/imaging/detector.py index 6f1d7d4..e322f40 100644 --- a/src/modules/imaging/detector.py +++ b/src/modules/imaging/detector.py @@ -3,6 +3,7 @@ from PIL import Image import numpy as np import cv2 +from cv2 import aruco from dep.labeller.loader import Vec2 from dep.labeller.benchmarks.detector import BoundingBox, LandingPadDetector @@ -29,3 +30,32 @@ def predict(self, image: Image.Image) -> Optional[BoundingBox]: return BoundingBox(Vec2(x, y), Vec2(w, h)) + +class ArucoDetector(): + + def predict(self, image: Image.Image) -> Optional[BoundingBox]: + img = cv2.cvtColor(np.array(image), cv2.COLOR_RGB2BGR) + + aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) + + params = cv2.aruco.DetectorParemeters() + + corners, ids, rejected = cv2.aruco.detectMarkers(img, aruco_dict, parameters=params) + + if ids: + for c in zip(corners, ids): + + pts = c[0] + + x_min = pts[:, 0].min() + x_max = pts[:, 0].max() + y_min = pts[:, 1].min() + y_max = pts[:, 1].max() + + x = (x_min + x_max) / 2 + y = (y_min + y_max) / 2 + w = (x_max - x_min) + h = (y_max - y_min) + + return BoundingBox(Vec2(x, y), Vec2(w, h)) + From 436066bd0699fff9f0b4a945b2b7bcfa9b813861 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 5 Dec 2025 17:41:09 -0700 Subject: [PATCH 15/16] Updated mavctl submodule to be the shepard submodule --- src/modules/mavctl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavctl b/src/modules/mavctl index d31619f..6cd7dc9 160000 --- a/src/modules/mavctl +++ b/src/modules/mavctl @@ -1 +1 @@ -Subproject commit d31619f66336623c91078c75abd0489138e670e5 +Subproject commit 6cd7dc96476bff4faf3292631adbba8936e61651 From 38fdd85bf6485c8bc077180ff28e04cad5ddc680 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 5 Dec 2025 17:46:35 -0700 Subject: [PATCH 16/16] Reverted update --- src/modules/mavctl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavctl b/src/modules/mavctl index 6cd7dc9..2fbb690 160000 --- a/src/modules/mavctl +++ b/src/modules/mavctl @@ -1 +1 @@ -Subproject commit 6cd7dc96476bff4faf3292631adbba8936e61651 +Subproject commit 2fbb69026fcb47539df5386c7cd80a01f71cf30f