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_
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)
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]
from udacidrone import Drone
from udacidrone.connection.mavlink_connection import MavlinkConnection
# Initiate a Mavlink connection
conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=True)
# Initialize a drone
drone = Drone(conn)
# Start receiving messages from the drone
drone.start()
# Take control and set the mode to guided
drone.take_control()
# Arms the rotors
drone.arm()
# Set the drone's home position
drone.set_home_position(drone.global_position[0], drone.global_position[1],
drone.global_position[2])
# Takeoff
drone.takeoff(3)
# Send a position (north, east, down, heading) command
drone.cmd_position(5, 0, 3, 0)
import numpy as np
import argparse
import matplotlib.pyplot as plt
from udacidrone import Drone
parser = argparse.ArgumentParser()
parser.add_argument('--logfile',
help='Path to the log file',
type=str,
default='Logs/TLog-manual.txt')
parser.add_argument('--output',
help='Path to the output figure',
type=str,
default='Logs/trajectory_manually_flying.png')
args = parser.parse_args()
filename = args.logfile
t_log = Drone.read_telemetry_data(filename)
# Time is always the first entry in the list
time = t_log['MsgID.LOCAL_POSITION'][0][:]
north = t_log['MsgID.LOCAL_POSITION'][1][:]
east = t_log['MsgID.LOCAL_POSITION'][2][:]
plt.figure(dpi=100)
plt.plot(north, east)
plt.xlabel('North (m)')
plt.ylabel('Ease (m)')
plt.savefig(args.output)
print('Plot of {} is written to {}'.format(args.logfile, args.output))
plt.close()
import argparse
import time
from udacidrone import Drone
from udacidrone.connection import MavlinkConnection, WebSocketConnection # noqa: F401
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('--port', type=int, default=3001, help='Port number')
parser.add_argument('--host', type=str, default='0.0.0.0', help="host address, i.e. '127.0.0.1'")
args = parser.parse_args()
conn = MavlinkConnection('tcp:{0}:{1}'.format(args.host, args.port), threaded=False, PX4=False)
# conn = WebSocketConnection('ws://{0}:{1}'.format(args.host, args.port))
drone = Drone(conn)
time.sleep(2)
drone.start()
import argparse
import time
from enum import Enum
from udacidrone import Drone
from udacidrone.connection import MavlinkConnection
conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=True)
drone = Drone(conn)
time.sleep(2)
print('starting drone...')
drone.start()
print('taking control...')
drone.take_control()
print('arming...')
drone.arm()
drone.set_home_position(drone.global_position[0], drone.global_position[1],
drone.global_position[2])
drone.takeoff(1)
time.sleep(5)
drone.cmd_position(0, 0, 3, 0)
time.sleep(5)
drone.cmd_position(0, 10, 3, 0)
time.sleep(5)
drone.cmd_position(10, 10, 3, 0)
time.sleep(5)
drone.cmd_position(10, 0, 3, 0)
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()
from udacidrone import Drone
from udacidrone.connection import MavlinkConnection
conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=True)
drone = Drone(conn)
drone.start()
drone.take_control()
drone.arm()
drone.set_home_position(drone.global_position[0],
drone.global_position[1],
drone.global_position[2])
drone.takeoff(30)
drone.cmd_position(5,5,5,5)
drone.stop()
from udacidrone import Drone
from udacidrone.connection import MavlinkConnection
conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=False, PX4=False)
drone = Drone(conn)
drone.connection.start()
drone.take_control()
drone.arm()
#drone.set_home_as_current_position()
drone.takeoff(3.0)
drone.cmd_position(2, 1, 3.0, 0)
drone.cmd_position(0, 0, 3, 0)
drone.land()
drone.disarm()
drone.release_control()
drone.stop()
from udacidrone import Drone
from udacidrone.connection import MavlinkConnection
import time
conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=True)
drone = Drone(conn)
drone.start()
drone.take_control()
drone.arm()
drone.set_home_position(drone.global_position[0], drone.global_position[1],
drone.global_position[2])
drone.takeoff(3)
square = [(10, 0, 5, 0), (10, 10, 5, 0), (0, 10, 5, 0), (0, 0, 5, 0)]
for sc in square:
drone.cmd_position(*sc)
time.sleep(3)
drone.takeoff(23)
drone.cmd_position(5, 0, 3, 0)
drone.land()
drone.release_control()
drone.disarm()
from udacidrone import Drone
from udacidrone.connection import MavlinkConnection
conn = MavlinkConnection('tcp:127.0.0.1:5760', threaded=True)
drone = Drone(conn)
drone.start()
drone.take_control()
drone.arm()
drone.takeoff(3)
drone.cmd_position(0, 0, 3, 0)
global_to_local(drone._longitude, drone._latitude, 0)
def point(p):
return np.array([p[0], p[1], 1.])
p1 = np.array([1., 2])
p2 = np.array([2, 3])
p3 = np.array([3, 3])
p4 = np.array([3, 4])
mat = np.vstack((point(p1), point(p2), point(p3)))
mat1 = np.vstack((point(p1), point(p2), point(p4)))
np.linalg.det(mat)
np.linalg.det(mat1)
