Convert mode code to DroneKit #23

flight/camera.py

@@ -4,15 +4,16 @@
 # Not worth to deal with this with the time crunch we are in
 
 import asyncio
+import math
 import json
 import logging
 import os
 from datetime import datetime
 
+import dronekit
 import gphoto2
 
 from flight.waypoint.calculate_distance import calculate_distance
-from state_machine.drone import Drone
 
 WAYPOINT_TOLERANCE: int = 1  # in meters
 
@@ -110,7 +111,7 @@ async def capture_photo(self, path: str = f"{os.getcwd()}/images/") -> tuple[str
 
     async def odlc_move_to(
         self,
-        drone: Drone,
+        drone: dronekit.Vehicle,
         latitude: float,
         longitude: float,
         altitude: float,
@@ -125,44 +126,48 @@ async def odlc_move_to(
 
         Parameters
         ----------
-        drone: System
+        drone : dronekit.Vehicle
             a drone object that has all offboard data needed for computation
-        latitude: float
+        latitude : float
             a float containing the requested latitude to move to
-        longitude: float
+        longitude : float
             a float containing the requested longitude to move to
-        altitude: float
+        altitude : float
             a float contatining the requested altitude to go to in meters
-        fast_param: float
+        fast_param : float
             a float that determines if the drone will take less time checking its precise location
             before moving on to another waypoint. If its 1, it will move at normal speed,
             if its less than 1(0.83), it will be faster.
-        take_photos: bool
+        take_photos : bool
             will take photos with the camera until the position has been reached
+        heading : float, default 0
+            the yaw in degrees (0 is north, positive is clockwise)
         """
-        if take_photos:
-            await drone.system.action.set_maximum_speed(5)
+        # Convert from MAVSDK to DroneKit yaw
+        # In MAVSDK Action.simple_goto, 0 yaw is north and 90 yaw is east
+        # In DroneKit Gimbal, 0 yaw is north and 90 yaw is west
+        heading = -heading % 360.0
 
         info: dict[str, dict[str, int | list[int | float] | float]] = {}
 
-        # get current altitude
-        async for terrain_info in drone.system.telemetry.home():
-            absolute_altitude: float = terrain_info.absolute_altitude_m
-            break
-
-        await drone.system.action.goto_location(
-            latitude, longitude, altitude + absolute_altitude, heading
+        drone.simple_goto(
+            dronekit.LocationGlobalRelative(latitude, longitude, altitude),
+            groundspeed=5.0 if take_photos else None,
         )
+        # TODO: See if setting the yaw like this actually works
+        drone.gimbal.rotate(drone.gimbal.pitch, drone.gimbal.roll, heading)
         location_reached: bool = False
         # First determine if we need to move fast through waypoints or need to slow down at each one
         # Then loops until the waypoint is reached
         while not location_reached:
             logging.info("Going to waypoint")
-            async for position in drone.system.telemetry.position():
+            while True:
+                position: dronekit.LocationGlobalRelative = drone.location.global_relative_frame
+
                 # continuously checks current latitude, longitude and altitude of the drone
-                drone_lat: float = position.latitude_deg
-                drone_long: float = position.longitude_deg
-                drone_alt: float = position.relative_altitude_m
+                drone_lat: float = position.lat
+                drone_long: float = position.lon
+                drone_alt: float = position.alt
 
                 total_distance: float = calculate_distance(
                     drone_lat, drone_long, drone_alt, latitude, longitude, altitude
@@ -180,11 +185,10 @@ async def odlc_move_to(
                 file_path: str
                 _full_path, file_path = await self.capture_photo()
 
-                async for euler in drone.system.telemetry.attitude_euler():
-                    roll_deg: float = euler.roll_deg
-                    pitch_deg: float = euler.pitch_deg
-                    yaw_deg: float = euler.yaw_deg
-                    break
+                attitude: dronekit.Attitude = drone.attitude
+                roll_deg: float = math.degrees(attitude.roll)
+                pitch_deg: float = math.degrees(attitude.pitch)
+                yaw_deg: float = math.degrees(attitude.yaw)
 
                 point: dict[str, dict[str, int | list[int | float] | float]] = {
                     file_path: {
@@ -212,7 +216,6 @@ async def odlc_move_to(
                 with open("flight/data/camera.json", "w", encoding="ascii") as camera:
                     json.dump(current_photos | info, camera)
 
-                await drone.system.action.set_maximum_speed(13.41)
             # tell machine to sleep to prevent constant polling, preventing battery drain
             await asyncio.sleep(1)
         return

flight/odlc_boundaries/README.md

@@ -1,7 +1,5 @@
 **ODLC Boundary Subdirectory**
 
-Execute - Executes the search path code
-
 Search Path - Creates a flight path for the drone to follow within a given set of coordinate points. Line parameters can be adjusted to increase or decrease space between lines if needed (Typically changed when camera settings like focal length are changed)
 
 This code is called when the ODLC state is engaged in the flight state machine.

flight/odlc_boundaries/search_path.py

@@ -8,9 +8,12 @@
 import logging
 import asyncio
 import utm
+
+import dronekit
+import dronekit_sitl
 from shapely.geometry import Polygon
-from mavsdk import System
-from execute import move_to
+
+from flight.waypoint.goto import move_to
 
 
 def latlon_to_utm(coords: dict[str, float]) -> dict[str, float]:
@@ -110,58 +113,75 @@ async def run() -> None:
         "Altitude": [85],
     }
 
-    # create a drone object
-    drone: System = System()
-    await drone.connect(system_address="udp://:14540")
-
-    # initilize drone configurations
-    await drone.action.set_takeoff_altitude(12)
-    await drone.action.set_maximum_speed(20)
-
-    # connect to the drone
-    logging.info("Waiting for drone to connect...")
-    async for state in drone.core.connection_state():
-        if state.is_connected:
-            logging.info("Drone discovered!")
-            break
-
-    logging.info("Waiting for drone to have a global position estimate...")
-    async for health in drone.telemetry.health():
-        if health.is_global_position_ok:
-            logging.info("Global position estimate ok")
-            break
-
-    logging.info("-- Arming")
-    await drone.action.arm()
-
-    logging.info("-- Taking off")
-    await drone.action.takeoff()
-
-    # wait for drone to take off
-    await asyncio.sleep(10)
-
-    # move to each waypoint in mission
-    point: int
-    for point in range(3):
-        logging.info("Moving")
-        await move_to(
-            drone, waypoint["lats"][point], waypoint["longs"][point], waypoint["Altitude"][0]
-        )
-
-    # infinite loop till forced disconnect
-    while True:
-        await asyncio.sleep(1)
+    sitl: dronekit_sitl.SITL = dronekit_sitl.start_default()
+    try:
+        # create a drone object
+        logging.info("Waiting for drone to connect...")
+        drone: dronekit.Vehicle = dronekit.connect(sitl.connection_string())
+        drone.wait_ready(True)
+
+        logging.info("Waiting for pre-arm checks to pass...")
+        while not drone.is_armable:
+            await asyncio.sleep(0.5)
+
+        logging.info("-- Arming")
+        drone.mode = dronekit.VehicleMode("GUIDED")
+        drone.armed = True
+        while drone.mode != "GUIDED" or not drone.armed:
+            await asyncio.sleep(0.5)
+
+        logging.info("-- Taking off")
+        drone.simple_takeoff(12.0)
+
+        # wait for drone to take off
+        await asyncio.sleep(10)
+
+        # move to each waypoint in mission
+        point: int
+        for point in range(3):
+            logging.info("Moving")
+            await move_to(
+                drone,
+                waypoint["lats"][point],
+                waypoint["longs"][point],
+                waypoint["Altitude"][0],
+                20.0,
+            )
+
+        # infinite loop till forced disconnect
+        while True:
+            await asyncio.sleep(1)
+    finally:
+        sitl.stop()
 
 
 if __name__ == "__main__":
     # Official Coordinates for Maryland
     data_search_area_boundary: list[dict[str, float]] = [
-        {"latitude": 38.3144070396263, "longitude": -76.54394394383165},  # Top Right Corner
-        {"latitude": 38.31430872867596, "longitude": -76.54397320409971},  # Right Midpoint
-        {"latitude": 38.31421041772561, "longitude": -76.54400246436776},  # Bottom Right Corner
-        {"latitude": 38.31461622313521, "longitude": -76.54516993186949},  # Top Left Corner
-        {"latitude": 38.31451966813249, "longitude": -76.54519982319357},  # Left Midpoint
-        {"latitude": 38.31442311312976, "longitude": -76.54522971451763},  # Bottom Left Corner
+        {
+            "latitude": 38.3144070396263,
+            "longitude": -76.54394394383165,
+        },  # Top Right Corner
+        {
+            "latitude": 38.31430872867596,
+            "longitude": -76.54397320409971,
+        },  # Right Midpoint
+        {
+            "latitude": 38.31421041772561,
+            "longitude": -76.54400246436776,
+        },  # Bottom Right Corner
+        {
+            "latitude": 38.31461622313521,
+            "longitude": -76.54516993186949,
+        },  # Top Left Corner
+        {
+            "latitude": 38.31451966813249,
+            "longitude": -76.54519982319357,
+        },  # Left Midpoint
+        {
+            "latitude": 38.31442311312976,
+            "longitude": -76.54522971451763,
+        },  # Bottom Left Corner
     ]
 
     # data_search_area_boundary: List[Dict[str, float]] = [

flight/waypoint/calculate_distance.py

@@ -3,14 +3,9 @@
 for calculating how far the drone is from the waypoint
 """
 
-import asyncio
-import logging
-
 import math
 import utm
 
-from mavsdk import System
-
 
 def calculate_distance(
     lat_deg_1: float,