Merge remote-tracking branch 'origin/develop' into feature/obstacle_a…

…voidance

.gitignore

@@ -156,4 +156,4 @@ vision_videos/
 *.m4a
 *.wmv
 *.mkv
-*.flv
+*.flv

.pre-commit-config.yaml

@@ -1,6 +1,10 @@
+ci:
+    autoupdate_schedule: monthly
+    skip: [pylint]
+
 repos:
--   repo: https://github.com/ambv/black
-    rev: 22.3.0
+-   repo: https://github.com/psf/black
+    rev: 22.12.0
     hooks:
     -   id: black
         additional_dependencies: ['click<8.1']
@@ -9,7 +13,7 @@ repos:
           ["--line-length=100"]
 
 -   repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.2.0  # Use the ref you want to point at
+    rev: v4.4.0  # Use the ref you want to point at
     hooks:
     -   id: check-json
     -   id: check-xml
@@ -25,14 +29,14 @@ repos:
         description: Forces line endings to the UNIX LF character
 
 -   repo: https://github.com/pre-commit/mirrors-mypy
-    rev: 'v0.942'
+    rev: 'v0.991'
     hooks:
     -   id: mypy
         args: ["--config-file=.mypy.ini"]
         additional_dependencies: ['numpy>=1.21.2']
 
 -   repo: https://github.com/PyCQA/pylint
-    rev: 'v2.13.7'
+    rev: 'v2.15.9'
     hooks:
     -   id: pylint
         name: pylint

flight/README.md

@@ -1,2 +1 @@
 # Flight
-

flight/Waypoint/Move_To_Test.py

@@ -0,0 +1,84 @@
+"""
+Main driver code for moving drone to each waypoint
+"""
+
+import asyncio
+import goto
+from mavsdk import System
+import logging
+from typing import List
+import sys
+
+
+async def run() -> None:
+    """
+    This function is a driver to test the goto function and runs through the
+    given waypoints in the lats and longs lists at the altitude of 100.
+    Makes the drone move to each location in the lats and longs arrays at the altitude of 100 and
+
+    Notes
+    -----
+    Currently has 3 values in each the Lats and Longs array and code is looped and will stay in that loop
+    until the drone has reached each of locations specified by the latitude and longitude and
+    continues to run until forced disconnect
+    """
+    # Put all latitudes, longitudes and altitudes into seperate arrays
+    lats: List[float] = [37.9008502, 37.9008129, 37.8964543]
+    longs: List[float] = [-91.6615228, -91.6615335, -91.6570381]
+
+    # 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
+    for point in range(len(lats)):
+        await goto.move_to(drone, lats[point], longs[point], 100, True)
+
+    # return home
+    logging.info("Last waypoint reached")
+    logging.info("Returning to home")
+    await drone.action.return_to_launch()
+    print("Staying connected, press Ctrl-C to exit")
+
+    # infinite loop till forced disconnect
+    while True:
+        await asyncio.sleep(1)
+
+
+if __name__ == "__main__":
+    """
+    Runs through the code until it has looped through each element of
+    the Lats and Longs array and the drone has arrived at each of them
+    """
+    try:
+        loop = asyncio.get_event_loop()
+        loop.run_until_complete(run())
+    except KeyboardInterrupt:
+        print("Program ended")
+        sys.exit(0)

flight/Waypoint/goto.py

@@ -0,0 +1,71 @@
+"""
+File containing the move_to function responsible
+for moving the drone to a certain waypoint and stopping there for 15 secs
+"""
+
+import asyncio
+from mavsdk import System
+import logging
+
+
+async def move_to(
+    drone: System, latitude: float, longitude: float, altitude: float, fast_param: 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
+        a drone object that has all offboard data needed for computation
+    latitude: float
+        a float containing the requested latitude to move to
+    longitude: float
+        a float containing the requested longitude to move to
+    altitude: float
+        a float contatining the requested altitude to go to (in feet)
+    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.
+    """
+
+    # converts feet into meters
+    altitude_in_meters = altitude * 0.3048
+
+    # 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_in_meters + absolute_altitude, 0)
+    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.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
+
+            #  accurately checks if location is reached and stops for 15 secs and then moves on.
+            if (
+                (round(drone_lat, int(6 * fast_param)) == round(latitude, int(6 * fast_param)))
+                and (
+                    round(drone_long, int(6 * fast_param)) == round(longitude, int(6 * fast_param))
+                )
+                and (round(drone_alt, 1) == round(altitude_in_meters, 1))
+            ):
+                location_reached = True
+                logging.info("arrived")
+                # sleeps for 15 seconds to give substantial time for the airdrop, can be changed later.
+                await asyncio.sleep(15)
+                break
+
+        # tell machine to sleep to prevent contstant polling, preventing battery drain
+        await asyncio.sleep(1)
+    return

flight/goto.py

@@ -0,0 +1,71 @@
+"""
+File containing the move_to function responsible
+for moving the drone to a certain waypoint and stopping there for 15 secs
+"""
+
+import asyncio
+from mavsdk import System
+import logging
+
+
+async def move_to(
+    drone: System, latitude: float, longitude: float, altitude: float, fast_param: 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
+        a drone object that has all offboard data needed for computation
+    latitude: float
+        a float containing the requested latitude to move to
+    longitude: float
+        a float containing the requested longitude to move to
+    altitude: float
+        a float contatining the requested altitude to go to (in feet)
+    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.
+    """
+
+    # converts feet into meters
+    altitude_in_meters = altitude * 0.3048
+
+    # 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_in_meters + absolute_altitude, 0)
+    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.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
+
+            #  accurately checks if location is reached and stops for 15 secs and then moves on.
+            if (
+                (round(drone_lat, int(6 * fast_param)) == round(latitude, int(6 * fast_param)))
+                and (
+                    round(drone_long, int(6 * fast_param)) == round(longitude, int(6 * fast_param))
+                )
+                and (round(drone_alt, 1) == round(altitude_in_meters, 1))
+            ):
+                location_reached = True
+                logging.info("arrived")
+                # sleeps for 15 seconds to give substantial time for the airdrop, can be changed later.
+                await asyncio.sleep(15)
+                break
+
+        # tell machine to sleep to prevent contstant polling, preventing battery drain
+        await asyncio.sleep(1)
+    return

flight/move_to_test.py

@@ -0,0 +1,106 @@
+"""
+Main driver code for moving drone to each waypoint
+"""
+
+import asyncio
+from mavsdk import System
+import logging
+import sys
+
+from flight import extract_gps
+from flight.Waypoint.goto import move_to
+
+
+async def run() -> None:
+    """
+    This function is a driver to test the goto function and runs through the
+    given waypoints in the lats and longs lists at the altitude of 100.
+    Makes the drone move to each location in the lats and longs arrays at the altitude of 100.
+
+    Notes
+    -----
+    Currently has 3 values in each the Lats and Longs array and code is looped and will stay in that loop
+    until the drone has reached each of locations specified by the latitude and longitude and
+    continues to run until forced disconnect
+    """
+    # Defining file path constant for extract_gps
+    PATH: str = "./data/waypoint_data.json"
+    # Defining system address
+    SYSTEM_ADDRESS: str = "udp://:14540"
+    # Defining altitude and speed
+    ALTITUDE: int = 12
+    SPEED: int = 20
+    # Defining fast_param constant
+    FAST_PARAM: float = 0.83
+
+    # Put all latitudes, longitudes and altitudes into seperate arrays
+    lats: list[float] = []
+    longs: list[float] = []
+    altitudes: list[float] = []
+
+    waypoint_data = extract_gps.extract_gps(PATH)
+    waypoints = waypoint_data["waypoints"]
+
+    waypoint: tuple[float, float, float]
+    for waypoint in waypoints:
+        lats.append(waypoint.latitude)
+        longs.append(waypoint.longitude)
+        altitudes.append(waypoint.altitude)
+
+    # create a drone object
+    drone: System = System()
+    await drone.connect(SYSTEM_ADDRESS)
+
+    # initilize drone configurations
+    await drone.action.set_takeoff_altitude(ALTITUDE)
+    await drone.action.set_maximum_speed(SPEED)
+
+    # 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(len(lats)):
+        await move_to(drone, lats[point], longs[point], 100, FAST_PARAM)
+
+    # return home
+    logging.info("Last waypoint reached")
+    logging.info("Returning to home")
+    await drone.action.return_to_launch()
+    print("Staying connected, press Ctrl-C to exit")
+
+    # infinite loop till forced disconnect
+    while True:
+        await asyncio.sleep(1)
+
+
+if __name__ == "__main__":
+    """
+    Runs through the code until it has looped through each element of
+    the Lats and Longs array and the drone has arrived at each of them
+    """
+    try:
+        loop = asyncio.get_event_loop()
+        loop.run_until_complete(run())
+    except KeyboardInterrupt:
+        logging.info("CTRL+C: Program ended")
+        sys.exit(0)