Fixed many files to work with new common hopefully

modules/data_merge/data_merge_worker.py

@@ -9,7 +9,7 @@
 from worker import worker_controller
 
 from modules import detections_and_odometry
-from modules import drone_odometry_local
+from modules import odometry_and_waypoint
 from modules import lidar_detection
 
 
@@ -38,7 +38,7 @@ def data_merge_worker(
             time.sleep(delay)
 
         try:
-            odometry: drone_odometry_local.DroneOdometryLocal = (
+            odometry: odometry_and_waypoint.OdometryAndWaypoint = (
                 odometry_input_queue.queue.get_nowait()
             )
 

modules/decision/decision.py

@@ -7,7 +7,7 @@
 
 from .. import decision_command
 from .. import detections_and_odometry
-from .. import drone_odometry_local
+from .. import odometry_and_waypoint
 
 
 class Decision:
@@ -29,7 +29,7 @@ def run_simple_decision(
         self,
         detections_and_odometries: "deque[detections_and_odometry.DetectionsAndOdometry]",
         proximity_limit: float,
-        current_flight_mode: drone_odometry_local.FlightMode,
+        current_flight_mode: odometry_and_waypoint.FlightMode,
     ) -> "tuple[bool, decision_command.DecisionCommand | None]":
         """
         Runs simple collision avoidance where drone will stop within a set distance of an object.
@@ -43,29 +43,29 @@ def run_simple_decision(
 
             if self.__command_requested:
                 if start_time - time.time() > self.command_timeout:
-                    if self.__last_command_sent == drone_odometry_local.FlightMode.STOPPED:
+                    if self.__last_command_sent == odometry_and_waypoint.FlightMode.LOITER:
                         return (
                             decision_command.DecisionCommand.create_stop_mission_and_halt_command()
                         )
-                    if self.__last_command_sent == drone_odometry_local.FlightMode.MOVING:
+                    if self.__last_command_sent == odometry_and_waypoint.FlightMode.AUTO:
                         return decision_command.DecisionCommand.create_resume_mission_command()
                 continue
 
-            if current_flight_mode == drone_odometry_local.FlightMode.STOPPED:
+            if current_flight_mode == odometry_and_waypoint.FlightMode.LOITER:
                 for detection in detections:
                     if detection.distance < proximity_limit:
                         return False, None
                 self.__command_requested = True
-                self.__last_command_sent = drone_odometry_local.FlightMode.MOVING
+                self.__last_command_sent = odometry_and_waypoint.FlightMode.AUTO
                 self.detections_and_odometries.clear()
                 start_time = time.time()
                 return decision_command.DecisionCommand.create_resume_mission_command()
 
-            if current_flight_mode == drone_odometry_local.FlightMode.MOVING:
+            if current_flight_mode == odometry_and_waypoint.FlightMode.AUTO:
                 for detection in detections:
                     if detection.distance < proximity_limit:
                         self.__command_requested = True
-                        self.__last_command_sent = drone_odometry_local.FlightMode.STOPPED
+                        self.__last_command_sent = odometry_and_waypoint.FlightMode.LOITER
                         self.detections_and_odometries.clear()
                         start_time = time.time()
                         return (

modules/detections_and_odometry.py

@@ -2,8 +2,8 @@
 LiDAR detection and local odometry data structure.
 """
 
-from . import drone_odometry_local
 from . import lidar_detection
+from . import odometry_and_waypoint
 
 
 class DetectionsAndOdometry:
@@ -17,7 +17,7 @@ class DetectionsAndOdometry:
     def create(
         cls,
         detections: "list[lidar_detection.LidarDetection]",
-        local_odometry: drone_odometry_local.DroneOdometryLocal,
+        local_odometry: odometry_and_waypoint.OdometryAndWaypoint,
     ) -> "tuple[bool, DetectionsAndOdometry | None]":
         """
         Combines lidar readings with local odometry
@@ -34,7 +34,7 @@ def __init__(
         self,
         create_key: object,
         detections: "list[lidar_detection.LidarDetection]",
-        local_odometry: drone_odometry_local.DroneOdometryLocal,
+        local_odometry: odometry_and_waypoint.OdometryAndWaypoint,
     ) -> None:
         """
         Private constructor, use create() method.

modules/flight_interface/conversions.py

@@ -4,13 +4,13 @@
 
 import pymap3d as pymap
 
-from .. import drone_odometry_local
-from ..common.mavlink.modules import drone_odometry
+from ..common.modules import position_local
+from ..common.modules import position_global
 
 
 def position_global_to_local(
-    global_position: drone_odometry.DronePosition, home_location: drone_odometry.DronePosition
-) -> "tuple[bool, drone_odometry_local.DronePositionLocal | None]":
+    global_position: position_global.PositionGlobal, home_location: position_global.PositionGlobal
+) -> "tuple[bool, position_local.PositionLocal | None]":
     """
     Converts global position (geodetic) to local position (NED).
     """
@@ -23,17 +23,17 @@ def position_global_to_local(
         home_location.altitude,
     )
 
-    result, local_position = drone_odometry_local.DronePositionLocal.create(north, east, down)
+    result, local_position = position_local.PositionLocal.create(north, east, down)
     if not result:
         return False, None
 
     return True, local_position
 
 
 def position_local_to_global(
-    local_position: drone_odometry_local.DronePositionLocal,
-    home_location: drone_odometry.DronePosition,
-) -> "tuple[bool, drone_odometry.DronePosition | None]":
+    local_position: position_local.PositionLocal,
+    home_location: position_global.PositionGlobal,
+) -> "tuple[bool, position_global.PositionGlobal| None]":
     """
     Converts local position (NED) to global position (geodetic).
     """
@@ -46,7 +46,7 @@ def position_local_to_global(
         home_location.altitude,
     )
 
-    result, global_position = drone_odometry.DronePosition.create(latitude, longitude, altitude)
+    result, global_position = position_global.PositionGlobal.create(latitude, longitude, altitude)
     if not result:
         return False, None
 

modules/flight_interface/flight_interface.py

@@ -5,9 +5,10 @@
 import math
 
 from modules import decision_command
-from modules import drone_odometry_local
-from ..common.mavlink.modules import drone_odometry
-from ..common.mavlink.modules import flight_controller
+from modules import odometry_and_waypoint
+from modules.common.modules.mavlink import flight_controller
+from modules.common.modules import position_global
+from modules.common.modules import position_local
 from . import conversions
 
 
@@ -33,7 +34,7 @@ def create(
         if not result:
             return False, None
 
-        result, home_location = controller.get_home_location(timeout_home)
+        result, home_location = controller.get_home_position(timeout_home)
         if not result:
             return False, None
 
@@ -67,8 +68,8 @@ def __init__(
         self,
         create_key: object,
         controller: flight_controller.FlightController,
-        home_location: drone_odometry.DronePosition,
-        first_waypoint: drone_odometry_local.DronePositionLocal,
+        home_location: position_global.PositionGlobal,
+        first_waypoint: position_local.PositionLocal,
         first_waypoint_distance_tolerance: float,
     ) -> None:
         """
@@ -84,7 +85,7 @@ def __init__(
         self.__run = False
 
     def __distance_to_first_waypoint_squared(
-        self, local_position: drone_odometry_local.DronePositionLocal
+        self, local_position: position_local.PositionLocal
     ) -> float:
         """
         Calculates the current distance in metres to the first waypoint (value is squared).
@@ -93,7 +94,7 @@ def __distance_to_first_waypoint_squared(
         delta_y = local_position.east - self.first_waypoint.east
         return delta_x**2 + delta_y**2
 
-    def run(self) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]":
+    def run(self) -> "tuple[bool, odometry_and_waypoint.OdometryAndWaypoint | None]":
         """
         Returns local drone odometry with timestamp.
         """
@@ -115,7 +116,7 @@ def run(self) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]":
         if not result:
             return False, None
 
-        flight_mode = drone_odometry_local.FlightMode(flight_mode.value)
+        flight_mode = odometry_and_waypoint.FlightMode(flight_mode.value)
 
         if not self.__run:
             distance_to_first_waypoint_squared = self.__distance_to_first_waypoint_squared(
@@ -126,8 +127,20 @@ def run(self) -> "tuple[bool, drone_odometry_local.DroneOdometryLocal | None]":
                 self.__run = True
                 print("Obstacle avoidance started!")
 
-        return drone_odometry_local.DroneOdometryLocal.create(
-            local_position, drone_orientation, flight_mode
+        result, next_waypoint = self.controller.get_next_waypoint()
+        if not result:
+            print("Error initializing flight interface: check waypoints are loaded.")
+            return False, None
+
+        result, next_waypoint_local = conversions.position_global_to_local(
+            next_waypoint, self.home_location
+        )
+
+        if not result:
+            return False, None
+
+        return odometry_and_waypoint.OdometryAndWaypoint.create(
+            local_position, drone_orientation, flight_mode, next_waypoint_local
         )
 
     def run_decision_handler(self, command: decision_command.DecisionCommand) -> bool:

modules/flight_interface/flight_interface_worker.py

@@ -6,7 +6,7 @@
 import queue
 
 from modules import decision_command
-from modules import drone_odometry_local
+from modules import odometry_and_waypoint
 from worker import queue_wrapper
 from worker import worker_controller
 from . import flight_interface
@@ -43,7 +43,7 @@ def flight_interface_worker(
 
         result, value = interface.run()
         if result:
-            if value.flight_mode == drone_odometry_local.FlightMode.MANUAL:
+            if value.flight_mode == odometry_and_waypoint.FlightMode.MANUAL:
                 print("Obstacle avoidance killed. Check flight mode.")
                 controller.request_exit()
                 break

modules/obstacles_and_odometry.py

@@ -2,7 +2,7 @@
 Obstacles and local odometry merged data structure.
 """
 
-from . import drone_odometry_local
+from modules import odometry_and_waypoint
 from . import obstacle
 
 
@@ -17,7 +17,7 @@ class ObstaclesAndOdometry:
     def create(
         cls,
         obstacles: "list[obstacle.Obstacle]",
-        local_odometry: drone_odometry_local.DroneOdometryLocal,
+        local_odometry: odometry_and_waypoint.OdometryAndWaypoint,
     ) -> "tuple[bool, ObstaclesAndOdometry | None]":
         """
         Combines obstacles with local odometry.
@@ -31,7 +31,7 @@ def __init__(
         self,
         create_key: object,
         obstacles: "list[obstacle.Obstacle]",
-        local_odometry: drone_odometry_local.DroneOdometryLocal,
+        local_odometry: odometry_and_waypoint.OdometryAndWaypoint,
     ) -> None:
         """
         Private constructor, use create() method.