|
| 1 | +#!/usr/bin/env python3 |
| 2 | + |
| 3 | +import asyncio |
| 4 | + |
| 5 | +from mavsdk import System |
| 6 | +from mavsdk.offboard import (PositionNedYaw, VelocityNedYaw, OffboardError) |
| 7 | + |
| 8 | +async def run(): |
| 9 | + drone = System() |
| 10 | + await drone.connect(system_address="udp://:14540") |
| 11 | + |
| 12 | + print("Waiting for drone to connect...") |
| 13 | + async for state in drone.core.connection_state(): |
| 14 | + if state.is_connected: |
| 15 | + print(f"Drone discovered with UUID: {state.uuid}") |
| 16 | + break |
| 17 | + |
| 18 | + print("-- Arming") |
| 19 | + await drone.action.arm() |
| 20 | + |
| 21 | + print("-- Setting initial setpoint") |
| 22 | + await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, 0.0, 0.0)) |
| 23 | + |
| 24 | + print("-- Starting offboard") |
| 25 | + try: |
| 26 | + await drone.offboard.start() |
| 27 | + except OffboardError as error: |
| 28 | + print(f"Starting offboard mode failed with error code: {error._result.result}") |
| 29 | + print("-- Disarming") |
| 30 | + await drone.action.disarm() |
| 31 | + return |
| 32 | + |
| 33 | + |
| 34 | + async def print_z_velocity(drone): |
| 35 | + async for odom in drone.telemetry.position_velocity_ned(): |
| 36 | + print(f"{odom.velocity.north_m_s} {odom.velocity.down_m_s}") |
| 37 | + |
| 38 | + asyncio.ensure_future(print_z_velocity(drone)) |
| 39 | + |
| 40 | + print("-- Go 0m North, 0m East, -10m Down within local coordinate system") |
| 41 | + #await drone.offboard.set_position_velocity_ned(PositionNedYaw(0.0, 0.0, -10.0, 0.0),VelocityNedYaw(0.0,0.0,-1.0,0.0)) |
| 42 | + await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -10.0, 0.0)) |
| 43 | + await asyncio.sleep(10) |
| 44 | + |
| 45 | + print("-- Go 10m North, 0m East, 0m Down within local coordinate system") |
| 46 | + await drone.offboard.set_position_ned(PositionNedYaw(50.0, 0.0, -10.0, 0.0)) |
| 47 | + #await drone.offboard.set_position_velocity_ned(PositionNedYaw(50.0, 0.0, -10.0, 0.0),VelocityNedYaw(1.0,0.0,0.0,0.0)) |
| 48 | + await asyncio.sleep(20) |
| 49 | + |
| 50 | + await drone.action.land() |
| 51 | + |
| 52 | + |
| 53 | + print("-- Stopping offboard") |
| 54 | + try: |
| 55 | + await drone.offboard.stop() |
| 56 | + except OffboardError as error: |
| 57 | + print(f"Stopping offboard mode failed with error code: {error._result.result}") |
| 58 | + |
| 59 | + |
| 60 | +if __name__ == "__main__": |
| 61 | + loop = asyncio.get_event_loop() |
| 62 | + loop.run_until_complete(run()) |
0 commit comments