Clean up current calculate_avoidance_velocity() function #16

flight/avoidance/obstacle_avoidance.py

@@ -4,7 +4,6 @@
 
 # pylint: disable=fixme
 
-import math
 import time
 
 import mavsdk
@@ -84,62 +83,36 @@ async def calculate_avoidance_velocity(
     # Sort obstacle positions with respect to time
     obstacle_positions.sort(key=lambda p: p.time)
 
-    # Check if inside avoidance radius, and return None if not
-    if (
-        math.hypot(
-            drone_position.utm_x - obstacle_positions[-1].utm_x,
-            drone_position.utm_y - obstacle_positions[-1].utm_y,
-            drone_position.altitude - obstacle_positions[-1].altitude,
-        )
-        > avoidance_radius
-    ):
-        return None
-
-    # Get velocity of drone
-    drone_velocity: mavsdk.telemetry.VelocityNed = await anext(drone.telemetry.velocity_ned())
-
-    # Convert drone velocity to Vector3 object
-    # Units don't change, only the type of the object
-    drone_velocity: Vector3 = Vector3.from_mavsdk_velocityned(drone_velocity)  # type: ignore
-
     # Estimate obstacle velocity
-    obstacle_velocity: Vector3 = Vector3(
-        obstacle_positions[-1].utm_y - obstacle_positions[-2].utm_y,
-        obstacle_positions[-1].utm_x - obstacle_positions[-2].utm_x,
-        obstacle_positions[-2].altitude - obstacle_positions[-1].altitude
-        # Altitudes reversed because we want downward velocity
+    obstacle_velocity: Vector3 = (
+        obstacle_positions[-1].to_vector3() - obstacle_positions[-2].to_vector3()
     ) / (obstacle_positions[-1].time - obstacle_positions[-2].time)
 
-    # Get velocity of our drone relative to the obstacle
-    relative_velocity: Vector3 = drone_velocity - obstacle_velocity
-
     # Extrapolate current obstacle position based on last known position and estimated velocity
     current_time: float = time.time()
     elapsed_time: float = current_time - obstacle_positions[-1].time
     estimated_obstacle_position: Point = Point(
-        utm_x=obstacle_positions[-1].utm_x + elapsed_time * drone_velocity.east,
-        utm_y=obstacle_positions[-1].utm_y + elapsed_time * drone_velocity.north,
-        utm_zone_number=obstacle_positions[-1].utm_zone_number,
-        utm_zone_letter=obstacle_positions[-1].utm_zone_letter,
-        altitude=obstacle_positions[-1].altitude - elapsed_time * drone_velocity.down,
-        time=current_time,
+        obstacle_positions[-1].utm_x + elapsed_time * obstacle_velocity.east,
+        obstacle_positions[-1].utm_y + elapsed_time * obstacle_velocity.north,
+        obstacle_positions[-1].utm_zone_number,
+        obstacle_positions[-1].utm_zone_letter,
+        obstacle_positions[-1].altitude - elapsed_time * obstacle_velocity.down,
+        current_time,
     )
 
-    # Get the relative velocity we want
-    desired_relative_velocity = (
-        avoidance_speed
-        * Vector3(
-            drone_position.utm_y - estimated_obstacle_position.utm_y,
-            drone_position.utm_x - estimated_obstacle_position.utm_x,
-            estimated_obstacle_position.altitude - drone_position.altitude
-            # Altitudes reversed because we want downward velocity
-        ).normalized()
+    # Get position difference
+    position_difference: Vector3 = (
+        drone_position.to_vector3() - estimated_obstacle_position.to_vector3()
     )
 
-    # Get the amount by which we should correct the drone's velocity
-    correction_velocity: Vector3 = desired_relative_velocity - relative_velocity
+    # Check if inside avoidance radius, and return None if not
+    if position_difference.length > avoidance_radius:
+        return None
+
+    # Get the relative velocity we want
+    desired_relative_velocity: Vector3 = avoidance_speed * position_difference.normalized()
 
     # Get the velocity at which the drone should move to avoid the obstacle
-    avoidance_velocity: Vector3 = drone_velocity + correction_velocity
+    avoidance_velocity: Vector3 = obstacle_velocity + desired_relative_velocity
 
     return avoidance_velocity

flight/avoidance/point.py

@@ -9,6 +9,8 @@
 import mavsdk.telemetry
 import utm
 
+from .vector.vector3 import Vector3
+
 # Input points are dicts with time and UTM coordinate data
 # May change in the future
 InputPoint = dict[str, float | str]
@@ -17,7 +19,7 @@
 @dataclass(slots=True)
 class Point:
     """
-    A point in 3D space
+    A point in 3D space and time
 
     Attributes
     ----------
@@ -120,3 +122,15 @@ def from_mavsdk_position(cls, position: mavsdk.telemetry.Position) -> Point:
             position.absolute_altitude_m,
             time.time(),
         )
+
+    def to_vector3(self) -> Vector3:
+        """
+        Converts a Point object to a Vector3 object
+
+        Returns
+        -------
+        A new Vector3 object in meters, dropping the
+        UTM zone number, UTM zone letter, and time
+        """
+
+        return Vector3(self.utm_y, self.utm_x, -self.altitude)