Precision landing

mavctl/messages/advanced.py

@@ -1,16 +1,20 @@
-import time 
-from messages import util
-from messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal
+import time
+from typing import Callable, Literal 
 from math import radians, atan
-from messages.util import Heading, LatLon_to_Distance
+
+from src.modules.mavctl.mavctl.messages.navigator import LandingTarget, Navigator
+from src.modules.mavctl.mavctl.messages import util
+from src.modules.mavctl.mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal
+from src.modules.mavctl.mavctl.messages.util import Heading, LatLon_to_Distance
 
 # This file is meant for implementing more advanced features in mavctl
 
 # This tests the wait target reached and has been verified to work
 
 def simple_goto_local(master, x, y, z): 
-
-    # Mimics simple_goto from dronekit, includes the right yaw angle so that the drone faces where it needs to go.
+    """Mimics simple_goto from dronekit, 
+    includes the right yaw angle so that 
+    the drone faces where it needs to go."""
 
     type_mask = master.generate_typemask([0, 1, 2, 9])
     angle = atan(y/x) 

mavctl/messages/location.py

@@ -26,6 +26,14 @@ def __init__(self, north, east, down):
         self.east = east
         self.down = down
 
+class Altitude(object):
+    def __init__(self, mono, amsl, local, relative, terrain, clearance):
+        self.mono = mono
+        self.amsl = amsl
+        self.local = local
+        self.relative = relative
+        self.terrain = terrain
+        self.clearance = clearance
 
 class Battery(object):
 

mavctl/messages/Navigator.py → mavctl/messages/navigator.py

@@ -1,20 +1,34 @@
-from pymavlink import mavutil
+from typing import Literal, Optional, Tuple
+from dataclasses import dataclass
 import time
-from messages import util
-from messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal, Velocity
 from math import sqrt
 
+from pymavlink import mavutil
+
+from src.modules.mavctl.mavctl.messages import util
+from src.modules.mavctl.mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal, Velocity, Altitude
+
+@dataclass
+class LandingTarget:
+    """
+    Data class to store position of landing target in MAV_FRAME_BODY_FRD frame.
+    FRD local frame aligned to the vehicle's attitude (x: Forward, y: Right, z: Down)
+    with an origin that travels with vehicle.
+    """
+    forward: float
+    right: float
+    altitude: float
+
 class Navigator:
 
     def __init__(self, mav):
         self.mav = mav
-        
+
         self.TOLERANCE_CE = 0.05
         # For checking if the target position has been reached. This is a coefficient which is multiplied by the distance travelled.
         # The reason why a coefficient method was chosen is because the position tolerance should be a function of distance as opposed to being a constant
         # This method is preferred so that what happened at AEAC 2025 doesnt happen again.
 
-
     def arm(self):
         """
         Arms the drone.
@@ -126,6 +140,26 @@ def get_global_position(self):
             alt = msg.alt / 1000
 
         return LocationGlobal(lat, lon, alt)
+
+    def get_geovector(point):
+        origin = self.get_global_position() 
+        distance = util.LatLon_to_Distance(point, origin)
+        bearing = radians(util.Heading(point, origin))
+
+        vectorX = distance * sin(bearing)
+        vectorY = distance * cos(bearing)
+
+        xRot = vectorX * cos(heading) - vectorY * sin(heading)
+        yRot = vectorx * sin(heading) + vectorY * cos(heading)
+
+        return (xRot, yRot)
+
+    def get_heading(self):
+        msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
+        if msg:
+            hdg = hdg / 100
+        return hdg
+
 
     def get_local_position(self):
         """
@@ -180,6 +214,20 @@ def get_velocity(self):
 
         return Velocity(north, east, down)
 
+    def get_altitude(self):
+        """
+        Gets the current system altitude, of various types
+        """
+        msg = self.mav.recv_match(type='ALTITUDE', blocking=True)
+        if msg:
+            mono = msg.altitude_monotonic
+            amsl = msg.altiude_amsl
+            local = msg.altitude_local
+            relative = msg.altitude_relative
+            terrain = msg.altitude_terrain
+            clearance = msg.bottom_clearance
+
+        return Altitude(mono, amsl, local, relative, terrain, clearance)
 
 
     def takeoff(self, altitude, pitch=15):
@@ -232,6 +280,60 @@ def vtol_takeoff(self, altitude, pitch=15):
 
         self.wait_target_reached(LocationLocal(0, 0, -altitude))
 
+    def land(self,
+             land_mode: Literal[0,1,2],
+             abort_alt: float = 0,
+             yaw_angle: Optional[float] = 0,
+             latitude: Optional[float] = 0,
+             longitude: Optional[float] = 0,
+             ):
+        """
+        Initiate a landing sequence.
+
+        Parameters:
+            abort_alt (float):
+                Minimum target altitude if landing is aborted
+                use system default if not specified. Units: meters.
+
+            land_mode (0, 1, 2):
+                Precision land mode:
+                    0: Precision land disabled
+                    1: opportunistic
+                    2: required 
+
+            yaw_angle (float, optional):
+                Desired yaw angle in degrees.
+                Default is to follow the current system yaw heading mode
+
+            latitude (float, optional):
+                Latitude in decimal degrees.
+
+            longitude (float, optional):
+                Longitude in decimal degrees.
+        """
+
+        # Runtime validation
+        if land_mode == 0 and (latitude is None or longitude is None): 
+            raise ValueError("specify latitude and longitude for disabled precision landing") 
+        self.mav.mav.command_long_send(
+                                        self.mav.target_system,
+                                        self.mav.target_component,
+                                        mavutil.mavlink.MAV_CMD_NAV_LAND,
+                                        0,
+                                        abort_alt,
+                                        land_mode,
+                                        0,
+                                        yaw_angle,
+                                        latitude,
+                                        longitude,
+                                        0)
+
+        current_position = self.get_local_position()
+        current_position.alt = 0
+        target = (current_position.north, current_position.down, 0)
+
+        self.wait_target_reached(current_position)
+
 
     def return_to_launch(self):
         print("MAVCTL: RTL")
@@ -249,11 +351,6 @@ def return_to_launch(self):
             0
         )
 
-
-
-
-
-
     def generate_typemask(self, keeps):
         # Generates typemask based on which values to be included
         mask = 0
@@ -451,3 +548,19 @@ def wait_target_reached_global(self, target, timeout = 30):
 
         return True
 
+    def broadcast_landing_target(self, landing_target: LandingTarget) -> None: 
+        """
+        This function broadcasts the position of the landing target in MAV_FRAME_BODY_FRD frame.
+        """
+
+        time_usec = int(time.time() * 1000000) # convert to microseconds
+
+        print(landing_target)
+        self.mav.mav.landing_target_send(time_usec=time_usec,
+                        target_num=1,
+                        frame=mavutil.mavlink.MAV_FRAME_BODY_FRD,
+                        angle_x=-landing_target.forward,
+                        angle_y=-landing_target.right,
+                        distance=landing_target.altitude,
+                        size_x=0,
+                        size_y=0)

mavctl/messages/util.py

@@ -1,7 +1,7 @@
 import math
 from math import radians, degrees, cos, sin, atan2, sqrt
 from pymavlink import mavutil
-from messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal
+from src.modules.mavctl.mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal
 
 
 def distance_to_target(target1, target2):
@@ -74,4 +74,3 @@ def Heading(point1, point2):
 
     return heading
 
-