diff --git a/modules/auto_landing/__init__.py b/modules/auto_landing/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/modules/auto_landing/auto_landing.py b/modules/auto_landing/auto_landing.py new file mode 100644 index 00000000..a8841062 --- /dev/null +++ b/modules/auto_landing/auto_landing.py @@ -0,0 +1,110 @@ +""" +Auto-landing script that calculates the necessary parameters +for use with LANDING_TARGET MAVLink command. +""" + +import math + +from .. import merged_odometry_detections +from ..common.modules.logger import logger + + +class AutoLandingInformation: + """ + Information necessary for the LANDING_TARGET MAVLink command. + """ + + def __init__(self, angle_x: float, angle_y: float, target_dist: float) -> None: + """ + Information necessary for the LANDING_TARGET MAVLink command. + + angle_x: Angle of the x coordinate of the bounding box within -π to π (rads). + angle_y: Angle of the y coordinate of the bounding box within -π to π (rads). + target_dist: Horizontal distance of vehicle to target (meters). + """ + + self.angle_x = angle_x + self.angle_y = angle_y + self.target_dist = target_dist + + +class AutoLanding: + """ + Auto-landing script that calculates the necessary parameters + for use with LANDING_TARGET MAVLink command. + """ + + __create_key = object() + + @classmethod + def create( + cls, + fov_x: float, + fov_y: float, + im_h: float, + im_w: float, + local_logger: logger.Logger, + ) -> "tuple [bool, AutoLanding | None ]": + """ + fov_x: The horizontal camera field of view in degrees. + fov_y: The vertical camera field of view in degrees. + im_w: Width of image. + im_h: Height of image. + + Returns an AutoLanding object. + """ + + return True, AutoLanding(cls.__create_key, fov_x, fov_y, im_h, im_w, local_logger) + + def __init__( + self, + class_private_create_key: object, + fov_x: float, + fov_y: float, + im_h: float, + im_w: float, + local_logger: logger.Logger, + ) -> None: + """ + Private constructor, use create() method. + """ + assert class_private_create_key is AutoLanding.__create_key, "Use create() method" + + self.fov_x = fov_x + self.fov_y = fov_y + self.im_h = im_h + self.im_w = im_w + self.__logger = local_logger + + def run( + self, odometry_detections: merged_odometry_detections.MergedOdometryDetections + ) -> "tuple[bool, AutoLandingInformation]": + """ + Calculates the x and y angles in radians of the bounding box based on its center. + + odometry_detections: A merged odometry dectections object. + + Returns an AutoLandingInformation object. + """ + + # TODO: Devise better algorithm to pick which detection to land at if several are detected + x_center, y_center = odometry_detections.detections[0].get_centre() + + angle_x = (x_center - self.im_w / 2) * (self.fov_x * (math.pi / 180)) / self.im_w + angle_y = (y_center - self.im_h / 2) * (self.fov_y * (math.pi / 180)) / self.im_h + + # This is height above ground level (AGL) + # down is how many meters down you are from home position, which is on the ground + height_agl = odometry_detections.odometry_local.position.down * -1 + + x_dist = math.tan(angle_x) * height_agl + y_dist = math.tan(angle_y) * height_agl + ground_hyp = (x_dist**2 + y_dist**2) ** 0.5 + target_to_vehicle_dist = (ground_hyp**2 + height_agl**2) ** 0.5 + + self.__logger.info( + f"X angle: {angle_x} Y angle: {angle_y}\nRequired horizontal correction: {ground_hyp} Distance from vehicle to target: {target_to_vehicle_dist}", + True, + ) + + return True, AutoLandingInformation(angle_x, angle_y, target_to_vehicle_dist) diff --git a/modules/auto_landing/auto_landing_worker.py b/modules/auto_landing/auto_landing_worker.py new file mode 100644 index 00000000..62890e04 --- /dev/null +++ b/modules/auto_landing/auto_landing_worker.py @@ -0,0 +1,66 @@ +""" +Auto-landing worker. +""" + +import os +import pathlib +import time + +from utilities.workers import queue_proxy_wrapper +from utilities.workers import worker_controller +from . import auto_landing +from ..common.modules.logger import logger + + +def auto_landing_worker( + fov_x: float, + fov_y: float, + im_h: float, + im_w: float, + period: float, + input_queue: queue_proxy_wrapper.QueueProxyWrapper, + output_queue: queue_proxy_wrapper.QueueProxyWrapper, + controller: worker_controller.WorkerController, +) -> None: + """ + Worker process. + + period: Wait time in seconds. + input_queue and output_queue are data queues. + controller is how the main process communicates to this worker process. + """ + + worker_name = pathlib.Path(__file__).stem + process_id = os.getpid() + result, local_logger = logger.Logger.create(f"{worker_name}_{process_id}", True) + if not result: + print("ERROR: Worker failed to create logger") + return + + # Get Pylance to stop complaining + assert local_logger is not None + + local_logger.info("Logger initialized", True) + + result, auto_lander = auto_landing.AutoLanding.create(fov_x, fov_y, im_h, im_w, local_logger) + + if not result: + local_logger.error("Worker failed to create class object", True) + return + + # Get Pylance to stop complaining + assert auto_lander is not None + + while not controller.is_exit_requested(): + controller.check_pause() + + input_data = input_queue.queue.get() + if input_data is None: + continue + + result, value = auto_lander.run(input_data) + if not result: + continue + + output_queue.queue.put(value) + time.sleep(period)