Skip to content

Commit

Permalink
Update Hoffman waypoint test to use DroneKit #23
Browse files Browse the repository at this point in the history
  • Loading branch information
RexBerry committed Oct 9, 2024
1 parent 0840ce5 commit 90d7e20
Showing 1 changed file with 23 additions and 22 deletions.
45 changes: 23 additions & 22 deletions flight/test_files/hoffman_waypoint_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,17 +6,15 @@
import logging
import sys

from mavsdk import System
import dronekit

from flight import extract_gps
from flight.waypoint.goto import move_to
from state_machine.drone import Drone

# Defining file path constant for extract_gps
MOVE_TO_TEST_PATH: str = "./data/waypoint_data.json"

# Defining system address
MOVE_TO_TEST_SYSTEM_ADDRESS: str = "udp://:14540"

# Defining altitude and speed
MOVE_TO_TEST_ALTITUDE: int = 12
MOVE_TO_TEST_SPEED: int = 20
Expand Down Expand Up @@ -51,31 +49,30 @@ async def run() -> None:
altitudes.append(waypoint.altitude)

# create a drone object
drone: System = System()
await drone.connect(MOVE_TO_TEST_SYSTEM_ADDRESS)
drone: Drone = Drone()
drone.use_sim_settings()
await drone.connect_drone()

# initilize drone configurations
await drone.action.set_takeoff_altitude(MOVE_TO_TEST_ALTITUDE)
await drone.action.set_maximum_speed(MOVE_TO_TEST_SPEED)
drone.vehicle.airspeed = MOVE_TO_TEST_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("Waiting for pre-arm checks to pass...")
while not drone.vehicle.is_armable:
await asyncio.sleep(0.5)

logging.info("-- Arming")
await drone.action.arm()
drone.vehicle.mode = dronekit.VehicleMode("GUIDED")
drone.vehicle.armed = True
while drone.vehicle.mode != "GUIDED" or not drone.vehicle.armed:
await asyncio.sleep(0.5)

logging.info("-- Taking off")
await drone.action.takeoff()
drone.vehicle.simple_takeoff(MOVE_TO_TEST_ALTITUDE)

# wait for drone to take off
while drone.vehicle.location.global_relative_frame.alt < MOVE_TO_TEST_ALTITUDE - 0.25:
await asyncio.sleep(1)

# wait for drone to take off
await asyncio.sleep(10)
Expand All @@ -88,7 +85,11 @@ async def run() -> None:
# return home
logging.info("Last waypoint reached")
logging.info("Returning to home")
await drone.action.return_to_launch()
drone.vehicle.mode = dronekit.VehicleMode("RTL")
while drone.vehicle.mode.name != "RTL":
await asyncio.sleep(0.5)
while drone.vehicle.system_status.state != "STANDBY":
await asyncio.sleep(0.5)
print("Staying connected, press Ctrl-C to exit")

# infinite loop till forced disconnect
Expand Down

0 comments on commit 90d7e20

Please sign in to comment.