async def f(connection_type):
global stop
stop = asyncio.Future()
asyncio.get_event_loop().add_signal_handler(signal.SIGALRM,
stop.set_result, None)
asyncio.get_event_loop().set_exception_handler(exception_handler)
if connection_type == ConnectionProtocol.WebSocket:
await ws_server()
elif connection_type == ConnectionProtocol.TCP:
await tcp_server()
await asyncio.sleep(0.05)
if connection_type == ConnectionProtocol.WebSocket:
conn = WebSocketConnection('{0}://{1}:{2}'.format(
ConnectionProtocol.WebSocket.value, HOST, PORT))
elif connection_type == ConnectionProtocol.TCP:
conn = MavlinkConnection('{0}:{1}:{2}'.format(
ConnectionProtocol.TCP.value, HOST, PORT),
threaded=True)
drone = Drone(conn)
signal.alarm(TIMEOUT)
await run_client(drone)
def create_drone(simulator=False):
# mav-link connection class:
conn = MavlinkConnection(
'tcp:127.0.0.1:8021',
threaded=True) #MavlinkConnection('tcp:127.0.0.1:5460', threaded=True)
#Drone-Class
drone_ = Drone(conn)
#Set Home
drone_.set_home_position(drone_.global_position[0],
drone_.global_position[1],
drone_.global_position[2])
return drone_
import numpy as np
import time
from udacidrone.frame_utils import global_to_local
from planning_utils import a_star, heuristic, create_grid
from udacidrone import Drone
from udacidrone.connection import MavlinkConnection
conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=True, PX4=False)
time.sleep(2)
drone = Drone(conn)
time.sleep(2)
drone.start()
time.sleep(2)
drone.take_control()
time.sleep(2)
drone.arm()
local position [ 6.97714961e+01 -1.32430816e+01 6.60000000e-02]
self.local_position [ 6.99566727e+01 -1.29187117e+01 6.64269775e-02]
local position [ 6.97714961e+01 -1.32430816e+01 6.60000000e-02]
self.local_position [ 6.99566727e+01 -1.29187117e+01 6.64269775e-02]
def test_drone_state_update():
f = BytesIO()
mav = mavlink.MAVLink(f)
c = Connection()
d = Drone(c)
# http://mavlink.org/messages/common/#HEARTBEAT
# msg = mav.heartbeat_encode(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0,
# mavutil.mavlink.MAV_STATE_ACTIVE)
# http://mavlink.org/messages/common/#GLOBAL_POSITION_INT
lat = 37.7749 * 1e7 # degrees * 1e7
lon = 122.4194 * 1e7 # degrees * 1e7
alt = 33.3 * 1000 # millimeters
vx = 12.3 * 100 # m/s * 100
vy = 14.3 * 100 # m/s * 100
vz = 18.3 * 100 # m/s * 100
hdg = 88 * 100 # degrees * 100
msg = mav.global_position_int_encode(time.time(), int(lat), int(lon),
int(alt), int(alt), int(vx), int(vy),
int(vz), int(hdg))
dispatch_message(c, msg)
# NOTE: the order switch of longitude and latitude
assert np.array_equal(
d.global_position,
np.array([float(lon) / 1e7,
float(lat) / 1e7,
float(alt) / 1000]))
assert np.array_equal(
d.local_velocity,
np.array([float(vx) / 100,
float(vy) / 100,
float(vz) / 100]))
# http://mavlink.org/messages/common#LOCAL_POSITION_NED
x = 10.
y = 20.
z = 30.
vx = 30.
vy = 22.
vz = 8.
msg = mav.local_position_ned_encode(time.time(), x, y, z, vx, vy, vz)
dispatch_message(c, msg)
assert np.array_equal(d.local_position, np.array([x, y, z]))
assert np.array_equal(d.local_velocity, np.array([vx, vy, vz]))
# http://mavlink.org/messages/common#HOME_POSITION
home_lat = 37.7749 * 1e7 # degrees * 1e7
home_lon = 122.4194 * 1e7 # degrees * 1e7
home_alt = 0. * 1000
msg = mav.home_position_encode(home_lat, home_lon, home_alt, 0, 0, 0, 0, 0,
0, 0)
dispatch_message(c, msg)
expect = np.array(
[float(home_lon) / 1e7,
float(home_lat) / 1e7,
float(home_alt) / 1000])
assert np.array_equal(d.global_home, expect)
# http://mavlink.org/messages/common#ATTITUDE_QUATERNION
# Calculate euler angles with http://quaternions.online/
q1 = 0.56098553
q2 = 0.43045933
q3 = -0.09229596
q4 = 0.70105738
expect_euler = np.deg2rad(np.array([30, -45, 90]))
rollspeed = 33
pitchspeed = 88
yawspeed = 47
msg = mav.attitude_quaternion_encode(time.time(), q1, q2, q3, q4,
rollspeed, pitchspeed, yawspeed)
dispatch_message(c, msg)
assert np.isclose(d.gyro_raw, np.array([rollspeed, pitchspeed,
yawspeed])).all()
assert np.isclose(d.attitude, expect_euler).all()
from udacidrone import Drone
from udacidrone.connection import MavlinkConnection
conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=False)
drone = Drone(conn, tlog_name="TLog-manual.txt")
drone.start()
drone.stop()