Using message_encode_decode to send MAVlink messages

main_2024.py

@@ -182,6 +182,10 @@ def main() -> int:
         mp_manager,
         QUEUE_MAX_SIZE,
     )
+    communications_to_flight_interface_queue = queue_proxy_wrapper.QueueProxyWrapper(
+        mp_manager,
+        QUEUE_MAX_SIZE,
+    )
     cluster_estimation_to_communications_queue = queue_proxy_wrapper.QueueProxyWrapper(
         mp_manager,
         QUEUE_MAX_SIZE,
@@ -272,7 +276,10 @@ def main() -> int:
             FLIGHT_INTERFACE_BAUD_RATE,
             FLIGHT_INTERFACE_WORKER_PERIOD,
         ),
-        input_queues=[flight_interface_decision_queue],
+        input_queues=[
+            flight_interface_decision_queue,
+            communications_to_flight_interface_queue,
+        ],
         output_queues=[
             flight_interface_to_data_merge_queue,
             flight_interface_to_communications_queue,
@@ -492,6 +499,7 @@ def main() -> int:
     cluster_estimation_to_communications_queue.fill_and_drain_queue()
     communications_to_main_queue.fill_and_drain_queue()
     flight_interface_decision_queue.fill_and_drain_queue()
+    communications_to_flight_interface_queue.fill_and_drain_queue()
 
     for manager in worker_managers:
         manager.join_workers()

modules/communications/communications.py

@@ -8,6 +8,7 @@
 from ..common.modules import position_global
 from ..common.modules import position_local
 from ..common.modules.logger import logger
+from ..common.modules.data_encoding import message_encoding_decoding
 from ..common.modules.mavlink import local_global_conversion
 
 
@@ -23,6 +24,7 @@ def create(
         cls,
         home_position: position_global.PositionGlobal,
         local_logger: logger.Logger,
+        worker_name: str,
     ) -> "tuple[True, Communications] | tuple[False, None]":
         """
         Logs data and forwards it.
@@ -32,13 +34,14 @@ def create(
         Returns: Success, class object.
         """
 
-        return True, Communications(cls.__create_key, home_position, local_logger)
+        return True, Communications(cls.__create_key, home_position, local_logger, worker_name)
 
     def __init__(
         self,
         class_private_create_key: object,
         home_position: position_global.PositionGlobal,
         local_logger: logger.Logger,
+        worker_name: str,
     ) -> None:
         """
         Private constructor, use create() method.
@@ -47,11 +50,12 @@ def __init__(
 
         self.__home_position = home_position
         self.__logger = local_logger
+        self.__worker_name = worker_name
 
     def run(
         self,
         objects_in_world: list[object_in_world.ObjectInWorld],
-    ) -> tuple[True, list[object_in_world.ObjectInWorld]] | tuple[False, None]:
+    ) -> tuple[True, list[bytes]] | tuple[False, None]:
 
         objects_in_world_global = []
         for object_in_world in objects_in_world:
@@ -87,4 +91,16 @@ def run(
 
         self.__logger.info(f"{time.time()}: {objects_in_world_global}")
 
-        return True, objects_in_world
+        encoded_position_global_objects = []
+        for object in objects_in_world_global:
+
+            result, message = message_encoding_decoding.encode_position_global(
+                f"{self.__worker_name}", object
+            )
+            if not result:
+                self.__logger.warning("Conversion from PositionGlobal to bytes failed", True)
+                return False, None
+
+            encoded_position_global_objects.append(message)
+
+        return True, encoded_position_global_objects

modules/communications/communications_worker.py

@@ -10,6 +10,7 @@
 from . import communications
 from utilities.workers import queue_proxy_wrapper
 from utilities.workers import worker_controller
+from ..common.modules.data_encoding import metadata_encoding_decoding
 from ..common.modules.logger import logger
 
 
@@ -49,7 +50,7 @@ def communications_worker(
 
     local_logger.info(f"Home position received: {home_position}", True)
 
-    result, comm = communications.Communications.create(home_position, local_logger)
+    result, comm = communications.Communications.create(home_position, local_logger, worker_name)
     if not result:
         local_logger.error("Worker failed to create class object", True)
         return
@@ -79,8 +80,18 @@ def communications_worker(
         if is_invalid:
             continue
 
-        result, value = comm.run(input_data)
+        result, list_of_messages = comm.run(input_data)
         if not result:
             continue
 
-        output_queue.queue.put(value)
+        result, metadata = metadata_encoding_decoding.encode_metadata(
+            f"{worker_name}", len(list_of_messages)
+        )
+        if not result:
+            local_logger.error("Failed to encode metadata", True)
+            continue
+
+        output_queue.queue.put(metadata)
+
+        for message in list_of_messages:
+            output_queue.queue.put(message)

modules/flight_interface/flight_interface.py

@@ -73,7 +73,7 @@ def get_home_position(self) -> position_global.PositionGlobal:
         """
         return self.__home_position
 
-    def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]":
+    def run(self, message: bytes) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]":
         """
         Returns a possible OdometryAndTime with current timestamp.
         """
@@ -103,6 +103,11 @@ def run(self) -> "tuple[bool, odometry_and_time.OdometryAndTime | None]":
 
         self.__logger.info(str(odometry_and_time_object), True)
 
+        result = self.controller.send_statustext_msg(message)
+        if not result:
+            self.__logger.error("Failed to send statustext message", True)
+            return False, None
+
         return True, odometry_and_time_object
 
     def apply_decision(self, cmd: decision_command.DecisionCommand) -> bool:

modules/flight_interface/flight_interface_worker.py

@@ -20,14 +20,17 @@ def flight_interface_worker(
     input_queue: queue_proxy_wrapper.QueueProxyWrapper,
     output_queue: queue_proxy_wrapper.QueueProxyWrapper,
     communications_output_queue: queue_proxy_wrapper.QueueProxyWrapper,
+    coordinates_input_queue: queue_proxy_wrapper.QueueProxyWrapper,
     controller: worker_controller.WorkerController,
 ) -> None:
     """
     Worker process.
 
     address, timeout is initial setting.
     period is minimum period between loops.
-    output_queue is the data queue.
+    input_queue and output_queue are the data queues.
+    communications_output_queue is a one time queue that sends communications worker the home address.
+    coordinates_input_queue provides the flight interface worker with a list of GPS coordinates.
     controller is how the main process communicates to this worker process.
     """
     # TODO: Error handling
@@ -62,7 +65,16 @@ def flight_interface_worker(
 
         time.sleep(period)
 
-        result, value = interface.run()
+        coordinate = coordinates_input_queue.queue.get()
+        if coordinate is None:
+            local_logger.info("Received type None, exiting")
+            break
+
+        if not isinstance(coordinate, bytes):
+            local_logger.warning(f"Skipping unexpected input: {coordinate}")
+            continue
+
+        result, value = interface.run(coordinate)
         if not result:
             continue