Convert most state code from MAVSDK to DroneKit (currently untested) #23

MissouriMRR · Sep 25, 2024 · ed9a1fc · ed9a1fc
1 parent 2d13a40
commit ed9a1fc
Show file tree

Hide file tree

Showing 11 changed files with 208 additions and 188 deletions.
diff --git a/flight/data/waypoint_data.json b/flight/data/waypoint_data.json
@@ -61,58 +61,7 @@
             }
         ]
     },
-    "waypoints": [
-        {
-            "latitude": 0,
-            "longitude": 0,
-            "altitude": 30.5
-        },
-        {
-            "latitude": 0,
-            "longitude": 0,
-            "altitude": 30.5
-        },
-        {
-            "latitude": 0,
-            "longitude": 0,
-            "altitude": 30.5
-        },
-        {
-            "latitude": 0,
-            "longitude": 0,
-            "altitude": 30.5
-        },
-        {
-            "latitude": 0,
-            "longitude": 0,
-            "altitude": 30.5
-        },
-        {
-            "latitude": 0,
-            "longitude": 0,
-            "altitude": 30.5
-        },
-        {
-            "latitude": 0,
-            "longitude": 0,
-            "altitude": 30.5
-        },
-        {
-            "latitude": 0,
-            "longitude": 0,
-            "altitude": 30.5
-        },
-        {
-            "latitude": 0,
-            "longitude": 0,
-            "altitude": 30.5
-        },
-        {
-            "latitude": 0,
-            "longitude": 0,
-            "altitude": 30.5
-        }
-    ],
+    "waypoints": [{"latitude": 38.31512674675237, "longitude": -76.54341021548873, "altitude": 57.761802254064335}, {"latitude": 38.31598492733403, "longitude": -76.55242495135296, "altitude": 57.53101888741179}, {"latitude": 38.316484959731305, "longitude": -76.55437539769747, "altitude": 45.4013304728879}, {"latitude": 38.318207092250745, "longitude": -76.54974661823606, "altitude": 96.97530128564443}, {"latitude": 38.31521086309381, "longitude": -76.54448525113388, "altitude": 57.563421931879034}, {"latitude": 38.31466748169034, "longitude": -76.5414824789626, "altitude": 52.885052034612514}, {"latitude": 38.31483043017296, "longitude": -76.54371343387862, "altitude": 48.81767362211545}, {"latitude": 38.31637308597654, "longitude": -76.5516362547742, "altitude": 118.88887819160173}, {"latitude": 38.31568031546251, "longitude": -76.55362503759268, "altitude": 39.48200477183701}, {"latitude": 38.31569390134637, "longitude": -76.54444973701482, "altitude": 93.13028477297108}, {"latitude": 38.3167470318657, "longitude": -76.55253410762896, "altitude": 93.29681720590949}, {"latitude": 38.31497997847468, "longitude": -76.5450499569001, "altitude": 93.85356935633227}, {"latitude": 38.31509559516682, "longitude": -76.55056182631999, "altitude": 114.26090554380512}, {"latitude": 38.31428231291275, "longitude": -76.54492488014353, "altitude": 25.962131817954955}, {"latitude": 38.316123334061864, "longitude": -76.55553874988334, "altitude": 109.05980925916651}, {"latitude": 38.317867202611836, "longitude": -76.54760204410559, "altitude": 68.0923189736532}, {"latitude": 38.31462197844122, "longitude": -76.54259714345547, "altitude": 72.20652998355327}, {"latitude": 38.31808432010314, "longitude": -76.54936601813543, "altitude": 23.84290056891879}, {"latitude": 38.315214814097516, "longitude": -76.5510454029886, "altitude": 57.42732474414447}, {"latitude": 38.316533151533, "longitude": -76.54897152139708, "altitude": 28.88858905781724}, {"latitude": 38.316603087199965, "longitude": -76.55574872774581, "altitude": 112.83894547777919}, {"latitude": 38.31539202279836, "longitude": -76.55119107211746, "altitude": 79.6698472154948}, {"latitude": 38.31584670966019, "longitude": -76.5554357531152, "altitude": 106.84255662883099}, {"latitude": 38.31421030348737, "longitude": -76.54118918014173, "altitude": 91.32538723874413}, {"latitude": 38.316161636575714, "longitude": -76.5438151199508, "altitude": 107.5255680162742}, {"latitude": 38.315363765708554, "longitude": -76.55233478216597, "altitude": 56.59581767374473}, {"latitude": 38.31569583322285, "longitude": -76.54519133796735, "altitude": 30.636820195648017}, {"latitude": 38.31834251472863, "longitude": -76.54655662486492, "altitude": 78.61364607540129}, {"latitude": 38.3140581948427, "longitude": -76.54539048196145, "altitude": 72.47628444772398}, {"latitude": 38.31848588067037, "longitude": -76.54866876642933, "altitude": 66.86516038844073}, {"latitude": 38.31674651131791, "longitude": -76.55104923808516, "altitude": 73.8531759033626}, {"latitude": 38.313501701784304, "longitude": -76.54145992547424, "altitude": 106.151515171501}, {"latitude": 38.31494510695642, "longitude": -76.54382272634888, "altitude": 52.666104110515356}, {"latitude": 38.317098885049646, "longitude": -76.55017121852315, "altitude": 53.19419028060346}, {"latitude": 38.31561731386353, "longitude": -76.55255238484861, "altitude": 93.44972362296609}, {"latitude": 38.315026514733916, "longitude": -76.55074935184867, "altitude": 40.64216603910177}, {"latitude": 38.316085938070685, "longitude": -76.54554839966094, "altitude": 109.09181738131713}, {"latitude": 38.31663643292306, "longitude": -76.55327742062485, "altitude": 95.23686106638627}, {"latitude": 38.31397339443596, "longitude": -76.54436592267048, "altitude": 68.33224317333142}, {"latitude": 38.31452296126658, "longitude": -76.54234680445576, "altitude": 30.090992497859705}, {"latitude": 38.31820532524588, "longitude": -76.55094257146689, "altitude": 27.615484038699176}, {"latitude": 38.31574785644684, "longitude": -76.5441811301492, "altitude": 85.71310237694877}, {"latitude": 38.31573923473714, "longitude": -76.55120145364364, "altitude": 72.00590186532642}, {"latitude": 38.31552058801431, "longitude": -76.54503581395701, "altitude": 44.59606475403423}, {"latitude": 38.314682589077115, "longitude": -76.54726151942505, "altitude": 66.72901539881474}, {"latitude": 38.31564842233868, "longitude": -76.55025356289136, "altitude": 109.72753626985117}, {"latitude": 38.31456723351206, "longitude": -76.54785959034069, "altitude": 40.174276242273024}, {"latitude": 38.31672319485429, "longitude": -76.54855245867192, "altitude": 52.42101680137705}, {"latitude": 38.31758763895303, "longitude": -76.54813148252572, "altitude": 90.43126857723499}, {"latitude": 38.3164176331506, "longitude": -76.546698905915, "altitude": 52.372196849713006}, {"latitude": 38.318536410828074, "longitude": -76.55023326641816, "altitude": 92.12802633771624}, {"latitude": 38.31540448239006, "longitude": -76.54256285287445, "altitude": 26.789353904218068}, {"latitude": 38.31530405122288, "longitude": -76.54714303643019, "altitude": 66.06555305300954}, {"latitude": 38.31511331988779, "longitude": -76.54060944938274, "altitude": 73.35780593151559}, {"latitude": 38.31638518886735, "longitude": -76.54821062569651, "altitude": 85.93798068161927}, {"latitude": 38.31475227172617, "longitude": -76.54862132666727, "altitude": 77.14696391114958}, {"latitude": 38.317967140991065, "longitude": -76.55177470312081, "altitude": 62.80378381602749}, {"latitude": 38.315248458206, "longitude": -76.54532659151016, "altitude": 118.56491690269712}, {"latitude": 38.31574779278646, "longitude": -76.55423018057833, "altitude": 52.240910636204276}, {"latitude": 38.31680633442682, "longitude": -76.55223448658677, "altitude": 67.04923372936912}, {"latitude": 38.31733982035793, "longitude": -76.54832179286711, "altitude": 35.222895524141684}, {"latitude": 38.31463770954804, "longitude": -76.54133063982678, "altitude": 59.01302998786507}, {"latitude": 38.31433368235518, "longitude": -76.54132674821824, "altitude": 110.57458094803755}, {"latitude": 38.315586115496046, "longitude": -76.54490796784965, "altitude": 99.68374343116389}, {"latitude": 38.314784411639785, "longitude": -76.5478026672805, "altitude": 85.86393117095736}, {"latitude": 38.31637682466238, "longitude": -76.5505471590834, "altitude": 117.9997845664227}, {"latitude": 38.315250845298245, "longitude": -76.54731187931183, "altitude": 66.32027106566035}, {"latitude": 38.31549916848741, "longitude": -76.54988356063478, "altitude": 81.4047181483788}, {"latitude": 38.31494610930144, "longitude": -76.54192201960586, "altitude": 110.45883945065138}, {"latitude": 38.31531636858013, "longitude": -76.540698954563, "altitude": 101.40395670754089}, {"latitude": 38.31817018266473, "longitude": -76.54755570409958, "altitude": 114.95834940530378}, {"latitude": 38.31780684309166, "longitude": -76.54695712779372, "altitude": 75.09121477615804}, {"latitude": 38.316080727001506, "longitude": -76.5448306643762, "altitude": 38.225866711351976}, {"latitude": 38.318520138499956, "longitude": -76.54586781288981, "altitude": 97.0675269486975}, {"latitude": 38.31731359267061, "longitude": -76.54984748065407, "altitude": 102.49420305568384}, {"latitude": 38.318103938824144, "longitude": -76.54588271404103, "altitude": 26.45070174074082}, {"latitude": 38.31731904633417, "longitude": -76.54461932539508, "altitude": 37.5446589731708}, {"latitude": 38.314659054392365, "longitude": -76.54853720928323, "altitude": 108.29778890494386}, {"latitude": 38.31748131760823, "longitude": -76.55043383217065, "altitude": 85.64993353581083}, {"latitude": 38.31644517208208, "longitude": -76.55125609572599, "altitude": 43.46232403139744}, {"latitude": 38.31512197550511, "longitude": -76.54320176659863, "altitude": 97.97447423229363}, {"latitude": 38.31660792474674, "longitude": -76.55271908754669, "altitude": 87.94282959319311}, {"latitude": 38.31855290469479, "longitude": -76.55010351918702, "altitude": 95.54057017639525}, {"latitude": 38.3181178485782, "longitude": -76.54891647667517, "altitude": 27.264872906378876}, {"latitude": 38.31506020725324, "longitude": -76.54734674133329, "altitude": 115.1502216816125}, {"latitude": 38.31712076567441, "longitude": -76.54677010693692, "altitude": 91.0107219082927}, {"latitude": 38.31486516062461, "longitude": -76.54342930881926, "altitude": 94.13429607066034}, {"latitude": 38.31445193317177, "longitude": -76.54129555205331, "altitude": 67.6832965903182}, {"latitude": 38.31861518553201, "longitude": -76.5488980006885, "altitude": 109.12791886468843}, {"latitude": 38.315695616954756, "longitude": -76.55462840668008, "altitude": 92.13357685637186}, {"latitude": 38.31621874552534, "longitude": -76.55560742848571, "altitude": 61.58651011570193}, {"latitude": 38.316766740405434, "longitude": -76.55341021500666, "altitude": 29.83660534490484}, {"latitude": 38.31548565528861, "longitude": -76.54333246630905, "altitude": 63.44072373463392}, {"latitude": 38.316834418575894, "longitude": -76.55570012782984, "altitude": 38.40198731044267}, {"latitude": 38.31659866962949, "longitude": -76.54486834765996, "altitude": 38.055014801625944}, {"latitude": 38.3173272268116, "longitude": -76.54690654556558, "altitude": 33.61423343452667}, {"latitude": 38.31599996651954, "longitude": -76.55407114572743, "altitude": 66.39398721674742}, {"latitude": 38.31615509776902, "longitude": -76.54958545433851, "altitude": 41.91478242300709}, {"latitude": 38.317128250543746, "longitude": -76.5472085397955, "altitude": 75.54409632777788}, {"latitude": 38.31657510447635, "longitude": -76.5498344353208, "altitude": 80.28117774035489}],
     "odlcAltitude": 24,
     "odlcHeading": 13,
     "odlcWaypoints": [

diff --git a/flight/waypoint/goto.py b/flight/waypoint/goto.py
@@ -6,7 +6,7 @@
 import asyncio
 import logging
 
-from mavsdk import System
+import dronekit
 
 from flight.waypoint.calculate_distance import calculate_distance
 
@@ -17,15 +17,17 @@
 # duplicate code disabled since we may want different functionality
 # for waypoints/odlcs search points
 # pylint: disable=duplicate-code
-async def move_to(drone: System, latitude: float, longitude: float, altitude: float) -> None:
+async def move_to(
+    drone: dronekit.Vehicle, latitude: float, longitude: float, altitude: float
+) -> None:
     """
     This function takes in a latitude, longitude and altitude and autonomously
     moves the drone to that waypoint. This function will also auto convert the altitude
     from feet to meters.
 
     Parameters
     ----------
-    drone: System
+    drone: Vehicle
         a drone object that has all offboard data needed for computation
     latitude: float
         a float containing the requested latitude to move to
@@ -34,33 +36,28 @@ async def move_to(drone: System, latitude: float, longitude: float, altitude: fl
     altitude: float
         a float containing the requested altitude to go to in meters
     """
-
-    # get current altitude
-    async for terrain_info in drone.telemetry.home():
-        absolute_altitude: float = terrain_info.absolute_altitude_m
-        break
-
-    await drone.action.goto_location(latitude, longitude, altitude + absolute_altitude, 0)
+    await drone.simple_goto(dronekit.LocationGlobalRelative)
     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
+    logging.info("Going to waypoint")
     while not location_reached:
-        logging.info("Going to waypoint")
-        async for position in drone.telemetry.position():
-            # 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
+        position: dronekit.LocationGlobalRelative = drone.location.global_relative_frame
+
+        # continuously checks current latitude, longitude and altitude of the drone
+        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
-            )
+        total_distance: float = calculate_distance(
+            drone_lat, drone_long, drone_alt, latitude, longitude, altitude
+        )
 
-            if total_distance < WAYPOINT_TOLERANCE:  # 6 meters = 19.685 feet.
-                location_reached = True
-                logging.info("Arrived %sm away from waypoint", total_distance)
-                break
+        if total_distance < WAYPOINT_TOLERANCE:  # 6 meters = 19.685 feet.
+            location_reached = True
+            logging.info("Arrived %sm away from waypoint", total_distance)
+            break
 
         # tell machine to sleep to prevent constant polling, preventing battery drain
-        await asyncio.sleep(1)
+        await asyncio.sleep(1.0)
     return
diff --git a/state_machine/drone.py b/state_machine/drone.py
@@ -1,6 +1,8 @@
 """Defines the Drone class for the state machine."""
 
-import mavsdk
+import asyncio
+
+import dronekit
 
 
 class Drone:
@@ -11,23 +13,30 @@ class Drone:
 
     Attributes
     ----------
-    system : mavsdk.System
-        The MAVSDK system object that controls the drone.
     address : str
-        The address used to connect to the drone when the `connect_drone()`
-        method is called.
+        The address used to connect to the drone.
+    is_connected
     odlc_scan : bool
         A boolean to tell if the odlc zone needs to be scanned, used the
         first run and if odlc needs to be scanned any other time
+    vehicle
+    _vehicle : dronekit.Vehicle | None
+        The Dronekit Vehicle object that controls the drone, or None if a connection
+        hasn't been made yet.
+
     Methods
     -------
-    __init__(address: str) -> None
+    __init__(connection_string: str) -> None
         Initialize a new Drone object, but do not connect to a drone.
     connect_drone(self) -> Awaitable[None]
         Connect to a drone.
+    is_connected(self) -> bool
+        Checks if a drone has been connected to.
+    vehicle(self) -> dronekit.Vehicle
+        Get the Dronekit Vehicle object owned by this Drone object.
     """
 
-    def __init__(self, address: str = "udp://:14540") -> None:
+    def __init__(self, address: str) -> None:
         """
         Initialize a new Drone object, but do not connect to a drone.
 
@@ -37,10 +46,45 @@ def __init__(self, address: str = "udp://:14540") -> None:
             The address of the drone to connect to when the `connect_drone()`
             method is called.
         """
-        self.system: mavsdk.System = mavsdk.System()
+        self._vehicle: dronekit.Vehicle | None = None
         self.address: str = address
         self.odlc_scan: bool = True
 
     async def connect_drone(self) -> None:
-        """Connect to a drone."""
-        await self.system.connect(system_address=self.address)
+        """Connect to a drone. This operation is idempotent."""
+        if self.is_connected:
+            return
+
+        vehicle: dronekit.Vehicle = dronekit.connect(self.address)
+        vehicle.wait_ready(True)  # TODO: modify dronekit and make it async
+        self._vehicle = vehicle
+
+    @property
+    def is_connected(self) -> bool:
+        """Checks if a drone has been connected to.
+
+        Returns
+        -------
+        bool
+            Whether this Drone object has connected to a drone.
+        """
+        return self._vehicle is not None
+
+    @property
+    def vehicle(self) -> dronekit.Vehicle:
+        """Get the Dronekit Vehicle object owned by this Drone object.
+
+        Returns
+        -------
+        dronekit.Vehicle
+            The Vehicle object owned by this Drone object.
+
+        Raises
+        ------
+        AttributeError
+            If a connection hasn't been made yet.
+        """
+        vehicle: dronekit.Vehicle | None = self._vehicle
+        if vehicle is None:
+            raise AttributeError("we haven't connected to the drone yet")
+        return vehicle