diff --git a/gettingstarted/GCS/GCS_Runner.py b/gettingstarted/GCS/GCS_Runner.py new file mode 100644 index 00000000..da148cac --- /dev/null +++ b/gettingstarted/GCS/GCS_Runner.py @@ -0,0 +1,73 @@ +import dronekit_sitl +import dronekit +import json +import argparse +import os +import threading +import time +import signal +import util +import logging +import simplegcs +from dronekit import connect, VehicleMode, LocationGlobalRelative + +# make sure you change this so that it's correct for your system +ARDUPATH = os.path.join('/', 'home', 'bayley', 'git', 'ardupilot') + + +def main(ardupath=None): + if ardupath is not None: + global ARDUPATH + ARDUPATH = ardupath + + + connect() + print("Registering Drones...") + vehicle1=addDrone([41.715446209367,-86.242847096132,0],"Frank") + + print("Taking off...") + vehicle1.simple_takeoff(10) + time.sleep(10) + print("Going somewhere...") + + gotoWaypoint(vehicle1,41.515446209367,-86.342847096132, 40,3) + + # point1 = LocationGlobalRelative(41.515446209367,-86.342847096132, 40) + # vehicle1.simple_goto(point1) + + # point2 = LocationGlobalRelative(41.515446209367, -86.342847096132, 40) + # vehicle2.simple_goto(point2) + +def connect(): + print("Connecting to Dronology...") + print("SITL path: "+ARDUPATH) + global GCS + GCS = simplegcs.SimpleGCS(ARDUPATH,"simplegcs") + GCS.connect() + time.sleep(10) + +def gotoWaypoint(vehicle,xcoord,ycoord,zcoord, airspeed=10): + vehicle.airspeed = airspeed + point = LocationGlobalRelative(xcoord,ycoord,zcoord) + vehicle.simple_goto(point) + +def addDrone(home, name=None): + vehicle = GCS.registerDrone(home,name) + time.sleep(5) + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(3) + print("Arming motors") + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + return vehicle + +if __name__ == '__main__': + ap = argparse.ArgumentParser() + #ap.add_argument('path_to_config', type=str, help='the path to the drone configuration file.') + ap.add_argument('--ardupath', type=str, default=ARDUPATH) + args = ap.parse_args() + main(ardupath=args.ardupath) diff --git a/gettingstarted/GCS/__init__.py b/gettingstarted/GCS/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/gettingstarted/GCS/simplegcs.py b/gettingstarted/GCS/simplegcs.py new file mode 100644 index 00000000..8a12bde7 --- /dev/null +++ b/gettingstarted/GCS/simplegcs.py @@ -0,0 +1,94 @@ +import dronekit_sitl +import dronekit +import json +import argparse +import os +import threading +import time +import signal +import util +import logging + +_LOG = logging.getLogger(__name__) +_LOG.setLevel(logging.INFO) + +fh = logging.FileHandler('main.log', mode='w') +fh.setLevel(logging.INFO) +formatter = logging.Formatter('| %(levelname)6s | %(funcName)8s:%(lineno)2d | %(message)s |') +fh.setFormatter(formatter) +_LOG.addHandler(fh) + +DO_CONT = False +MESSAGE_FREQUENCY=1.0 + +class SimpleGCS: + sitls = [] + vehicles = {} + + def __init__(self,ardupath,g_id="default_groundstation"): + self.g_id=g_id + self.ardupath=ardupath + + def registerDrone(self, home,name,virtual=True): + if name is None: + name = get_vehicle_id(len(self.vehicles)) + + if virtual: + vehicle, sitl = self.connect_virtual_vehicle(len(self.vehicles), home) + self.sitls.append(sitl) + else: + vehicle = self.connect_physical_vehicle(home) + handshake = util.DroneHandshakeMessage.from_vehicle(vehicle, self.dronology._g_id,name) + + self.vehicles[name]=vehicle + self.dronology.send(str(handshake)) + print("New drone registered.."+handshake.__str__()) + return vehicle + + def connect(self): + self.dronology = util.Connection(None, "localhost", 1234,self.g_id) + self.dronology.start() + global DO_CONT + DO_CONT = True + w0 = threading.Thread(target=state_out_work, args=(self.dronology, self.vehicles)) + w0.start() + + def connect_virtual_vehicle(self,instance, home): + home_ = tuple(home) + (0,) + home_ = ','.join(map(str, home_)) + sitl_defaults = os.path.join(self.ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') + sitl_args = ['-I{}'.format(instance), '--home', home_, '--model', '+', '--defaults', sitl_defaults] + sitl = dronekit_sitl.SITL(path=os.path.join(self.ardupath, 'build', 'sitl', 'bin', 'arducopter')) + sitl.launch(sitl_args, await_ready=True) + + tcp, ip, port = sitl.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + + vehicle = dronekit.connect(conn_string) + vehicle.wait_ready(timeout=120) + + return vehicle, sitl + + def connect_physical_vehicle(self, home): + + vehicle = dronekit.connect(home, wait_ready=True) + vehicle.wait_ready(timeout=120) + + return vehicle + + +def get_vehicle_id(i): + return 'drone{}'.format(i) + + +def state_out_work(dronology, vehicles): + while DO_CONT: + # for i, v in enumerate(vehicles): + for name, v in vehicles.iteritems(): + state = util.StateMessage.from_vehicle(v,dronology._g_id,name) + state_str = str(state) + _LOG.info(state_str) + dronology.send(state_str) + + time.sleep(MESSAGE_FREQUENCY) diff --git a/gettingstarted/GCS/util.py b/gettingstarted/GCS/util.py new file mode 100644 index 00000000..c213cc6c --- /dev/null +++ b/gettingstarted/GCS/util.py @@ -0,0 +1,173 @@ +import json +import os +import threading +import socket +import time +from boltons import socketutils + + +class DronologyMessage(object): + def __init__(self, m_type,g_id, uav_id, data): + self.m_type = m_type + self.g_id=g_id + self.uav_id = uav_id + self.data = data + + def __str__(self): + return json.dumps({'type': self.m_type, + 'sendtimestamp': long(round(time.time() * 1000)), + 'uavid': str(self.uav_id), + 'groundstationid': str(self.g_id), + 'data': self.data}) + + def __repr__(self): + return str(self) + + +class DroneHandshakeMessage(DronologyMessage): + def __init__(self, g_id,uav_id, data, p2sac='../cfg/sac.json'): + super(DroneHandshakeMessage, self).__init__('handshake',g_id, uav_id, data) + self.p2sac = p2sac + + @classmethod + def from_vehicle(cls, vehicle,g_id, v_id, p2sac='../cfg/sac.json'): + battery = { + 'voltage': vehicle.battery.voltage, + 'current': vehicle.battery.current, + 'level': vehicle.battery.level, + } + + lla = vehicle.location.global_relative_frame + data = { + 'home': {'x': lla.lat, + 'y': lla.lon, + 'z': lla.alt}, + 'safetycase': json.dumps({})} + return cls(g_id,v_id, data) + + +class StateMessage(DronologyMessage): + def __init__(self, g_id, uav_id, data): + super(StateMessage, self).__init__('state',g_id, uav_id, data) + + @classmethod + def from_vehicle(cls, vehicle,g_id, v_id, **kwargs): + lla = vehicle.location.global_relative_frame + att = vehicle.attitude + vel = vehicle.velocity + battery = { + 'voltage': vehicle.battery.voltage, + 'current': vehicle.battery.current, + 'level': vehicle.battery.level, + } + data = { + 'location': {'x': lla.lat, 'y': lla.lon, 'z': lla.alt}, + 'attitude': {'x': att.roll, 'y': att.pitch, 'z': att.yaw}, + 'velocity': {'x': vel[0], 'y': vel[1], 'z': vel[2]}, + 'status': vehicle.system_status.state, + 'heading': vehicle.heading, + 'armable': vehicle.is_armable, + 'airspeed': vehicle.airspeed, + 'groundspeed': vehicle.airspeed, + 'armed': vehicle.armed, + 'mode': vehicle.mode.name, + 'batterystatus': battery + } + + return cls(g_id,v_id, data) + +class Connection: + _WAITING = 1 + _CONNECTED = 2 + _DEAD = -1 + + def __init__(self, msg_queue=None, addr='localhost', port=1234, g_id='default_groundstation'): + self._g_id = g_id + self._msgs = msg_queue + self._addr = addr + self._port = port + self._sock = None + self._conn_lock = threading.Lock() + self._status = Connection._WAITING + self._status_lock = threading.Lock() + self._msg_buffer = '' + + def get_status(self): + with self._status_lock: + return self._status + + def set_status(self, status): + with self._status_lock: + self._status = status + + def is_connected(self): + return self.get_status() == Connection._CONNECTED + + def start(self): + threading.Thread(target=self._work).start() + + def stop(self): + self.set_status(Connection._DEAD) + + def send(self, msg): + success = False + with self._conn_lock: + if self._status == Connection._CONNECTED: + try: + self._sock.send(msg) + self._sock.send(os.linesep) + success = True + except Exception as e: + print('failed to send message! ({})'.format(e)) + + return success + + def _work(self): + """ + Main loop. + 1. Wait for a connection + 2. Once connected, wait for commands from dronology + 3. If connection interrupted, wait for another connection again. + 4. Shut down when status is set to DEAD + :return: + """ + cont = True + while cont: + status = self.get_status() + if status == Connection._DEAD: + # Shut down + cont = False + elif status == Connection._WAITING: + # Try to connect, timeout after 10 seconds. + try: + sock = socket.create_connection((self._addr, self._port), timeout=5.0) + self._sock = socketutils.BufferedSocket(sock) + handshake = json.dumps({'type': 'connect', 'groundstationid': self._g_id}) + self._sock.send(handshake) + self._sock.send(os.linesep) + self.set_status(Connection._CONNECTED) + except socket.error as e: + print('Socket error ({})'.format(e)) + time.sleep(10.0) + else: + # Receive messages + try: + msg = self._sock.recv_until(os.linesep, timeout=0.1) + if self._msgs is not None: + self._msgs.append(msg) + except socket.timeout: + pass + except socket.error as e: + print('connection interrupted! ({})'.format(e)) + self._sock.shutdown(socket.SHUT_RDWR) + self._sock.close() + self._sock = None + self.set_status(Connection._WAITING) + time.sleep(20.0) + + if self._sock is not None: + print('Shutting down socket.') + self._sock.shutdown(socket.SHUT_WR) + print('Closing socket.') + self._sock.close() + return \ No newline at end of file diff --git a/gettingstarted/NED/__init__.py b/gettingstarted/NED/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/gettingstarted/NED/flight_plotter.py b/gettingstarted/NED/flight_plotter.py new file mode 100644 index 00000000..620a9f90 --- /dev/null +++ b/gettingstarted/NED/flight_plotter.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +Graph Utilities for plotting longitude and latitude of flight results +""" +from pymavlink import mavutil +import math +import re +import numpy as np +import matplotlib.pyplot as plt + + +class Location: + def __init__(self,lat=0.0,lon=0.0): + """ Create a new point at the origin """ + self.lat = lat + self.lon = lon + +class CoordinateLogger: + def __init__(self): + self.lat_array = [] + self.lon_array = [] + + def add_data(self,latitude,longitude): + """ + + :rtype: object + """ + self.lat_array.append([]) + self.lon_array.append([]) + self.lat_array[-1].append(latitude) + self.lon_array[-1].append(longitude) + + +############################################################################################## +# Provides Graph Plotting functionality +############################################################################################## +class GraphPlotter: + def __init__(self,lat1_array,lon1_array,lat2_array,lon2_array,xlabel="",ylabel="",title=""): + self.lat1_array=lat1_array + self.lon1_array=lon1_array + self.lat2_array=lat2_array + self.lon2_array=lon2_array + self.xlegend = xlabel + self.ylegend=ylabel + self.title=title + self.marker_lat = 0 + self.marker_lon = 0 + + def add_marker(self,markerlat,markerlon): + self.marker_lat = markerlat + self.marker_lon = markerlon + + def scatter_plot(self): + plt.plot(self.lat1_array,self.lon1_array,linewidth=10,color='gray') + plt.plot(self.lat2_array,self.lon2_array,linewidth=4,color='blue') + plt.xlabel(self.xlegend) + plt.ylabel(self.ylegend) + plt.title(self.title) + if self.marker_lat != 0: + plt.plot([self.marker_lat], [self.marker_lon], "ro", markersize=22) + plt.show() + diff --git a/gettingstarted/NED/fly_a_circle.py b/gettingstarted/NED/fly_a_circle.py new file mode 100644 index 00000000..c6516230 --- /dev/null +++ b/gettingstarted/NED/fly_a_circle.py @@ -0,0 +1,239 @@ +##!/ usr / bin / env python +# -*- coding: utf-8 -*- + +""" +Modified from 3DR simple_goto.py +Added code for flying using NED Velocity Vectors - Jane Cleland-Huang 1/15 +""" +import math +import os +import time +from math import sin, cos, atan2, radians, sqrt + +from dronekit import Vehicle, connect, VehicleMode, LocationGlobalRelative +from dronekit_sitl import SITL + +from flight_plotter import Location, CoordinateLogger, GraphPlotter +from ned_utilities import ned_controller + +def connect_virtual_vehicle(instance, home): + sitl = SITL() + sitl.download('copter', '3.3', verbose=True) + instance_arg = '-I%s' %(str(instance)) + home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2])) + speedup_arg = '--speedup=4' + sitl_args = [instance_arg, '--model', 'quad', home_arg, speedup_arg] + #sitl_args = [instance_arg, '--model', 'quad', home_arg] + sitl.launch(sitl_args, await_ready=True) + tcp, ip, port = sitl.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + print('Connecting to vehicle on: %s' % conn_string) + + vehicle = connect(conn_string) + vehicle.wait_ready(timeout=120) + print("Reached here") + return vehicle, sitl + + +################################################################################################ +# ARM and TAKEOFF +################################################################################################ + +# function: arm and takeoff +# parameters: target altitude (e.g., 10, 20) +# returns: n/a + +def arm_and_takeoff(aTargetAltitude): + """ + Arms vehicle and fly to aTargetAltitude. + """ + + print("Basic pre-arm checks") + # Don't try to arm until autopilot is ready + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(1) + + print("home: " + str(vehicle.location.global_relative_frame.lat)) + + print("Arming motors") + # Copter should arm in GUIDED mode + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + # Confirm vehicle armed before attempting to take off + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + print("Taking off!") + vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude + + while True: + # Break and return from function just below target altitude. + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * .95: + print("Reached target altitude") + break + time.sleep(1) + + +################################################################################################ +# function: Get distance in meters +# parameters: Two global relative locations +# returns: Distance in meters +################################################################################################ +def get_distance_meters(locationA, locationB): + # approximate radius of earth in km + R = 6373.0 + + lat1 = radians(locationA.lat) + lon1 = radians(locationA.lon) + lat2 = radians(locationB.lat) + lon2 = radians(locationB.lon) + + dlon = lon2 - lon1 + dlat = lat2 - lat1 + + a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2 + c = 2 * atan2(sqrt(a), sqrt(1 - a)) + + distance = (R * c) * 1000 + + # print("Distance (meters):", distance) + return distance + + +################################################################################################ +# Fly to a target location using a waypoint +################################################################################################ +def fly_to(vehicle, targetLocation, groundspeed): + print("Flying from: " + str(vehicle.location.global_frame.lat) + "," + str( + vehicle.location.global_frame.lon) + " to " + str(targetLocation.lat) + "," + str(targetLocation.lon)) + vehicle.groundspeed = groundspeed + currentTargetLocation = targetLocation + vehicle.simple_goto(currentTargetLocation) + remainingDistance = get_distance_meters(currentTargetLocation, vehicle.location.global_frame) + + while vehicle.mode.name == "GUIDED": + remainingDistance = get_distance_meters(currentTargetLocation, vehicle.location.global_frame) + print(remainingDistance) + if remainingDistance < 1: + print("Reached target") + break + time.sleep(1) + +################################################################################################ +# Given the GPS coordinates of the center of a circle, and a degree (0-360) and radius +# return a point on the circumference. +################################################################################################ +def point_on_circle(radius, angle_indegrees, latitude, longitude): + # Convert from degrees to radians + lon = (radius * math.cos(angle_indegrees * math.pi / 180)) + longitude + lat = (radius * math.sin(angle_indegrees * math.pi / 180)) + latitude + return Location(lat, lon) + + +############################################################################################ +# Main functionality +############################################################################################ + +vehicle, sitl = connect_virtual_vehicle(1,([41.7144367,-86.2417136,0])) #"41.7144367,-86.2417136,221,0" +start = time.time() + +arm_and_takeoff(10) + +# Get current location of vehicle and establish a conceptual circle around it for flying +center = Location(vehicle.location.global_relative_frame.lat, + vehicle.location.global_relative_frame.lon) +radius = .0001 +angle = 0 #Starting angle + +# Fly to starting position using waypoint +currentLocation = center +startingPos = point_on_circle(radius*.95, angle+1, center.lat, center.lon) # Close to first position on circle perimeter +firstTargetPosition = LocationGlobalRelative(startingPos.lat, startingPos.lon, 10) +fly_to(vehicle, firstTargetPosition, 10) + +# Establish a starting angle to compute next position on circle +angle = 0 + +# Establish an instance of CoordinateLogger +log1 = CoordinateLogger() + +# Create a NedController +nedcontroller = ned_controller() +waypoint_goto = False + +# Fly from one point on the circumference to the next +while angle <= 360: + + # For NED flying compute the NED Velocity vector + print("\nNew target " + str(angle)) + nextTarget = (point_on_circle(radius, angle, center.lat, center.lon)) + currentLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon) + distance_to_target = get_distance_meters(currentLocation, nextTarget) + closestDistance=distance_to_target + ned = nedcontroller.setNed(currentLocation, nextTarget) + + # Keep going until target is reached + while distance_to_target > 1: + currentLocation = Location(vehicle.location.global_relative_frame.lat, + vehicle.location.global_relative_frame.lon) + distance_to_target = get_distance_meters(currentLocation, nextTarget) + print ('Current Pos: (' + str(currentLocation.lat) + "," + str(currentLocation.lon) + + ') Target Pos: ' + str(nextTarget.lat) + ' Target lon: ' + str(nextTarget.lon) + ' Distance: ' + str( + distance_to_target) + " NED: " + str(ned.north) + " " + str(ned.east)) + + if distance_to_target > closestDistance: #Prevent unwanted fly-by + break + else: + closestDistance = distance_to_target + + # Needed if the ned vectors don't take you to the final target i.e., + # You'll need to create another NED. You could reuse the previous one + # but recomputing it can mitigate small errors which otherwise build up. + currentLocation = Location(vehicle.location.global_relative_frame.lat, + vehicle.location.global_relative_frame.lon) + ned = nedcontroller.setNed(currentLocation, nextTarget) + + # Either fly using waypoints or NEDs + if waypoint_goto == True: + firstTargetPosition = LocationGlobalRelative(nextTarget.lat, nextTarget.lon, 10) + fly_to(vehicle, firstTargetPosition, 10) + else: + nedcontroller.send_ned_velocity(ned.north, ned.east, ned.down, 1, vehicle) + + #Log data + log1.add_data(currentLocation.lat,currentLocation.lon) + + angle = angle + 2 + +print("Returning to Launch") +vehicle.mode = VehicleMode("RTL") +# Close vehicle object before exiting script +print("Close vehicle object") +vehicle.close() + +# Shut down simulator if it was started. +if sitl is not None: + sitl.stop() + +end = time.time() +print(end - start) + +########################################################################### +# Create the target coordinates +########################################################################### +log2 = CoordinateLogger() +angle = 0 +while angle <= 360: + point = (point_on_circle(radius, angle, center.lat, center.lon)) + log2.add_data(point.lat,point.lon) + angle = angle + 1 + +########################################################################### +# Plot graph +########################################################################### +plotter = GraphPlotter(log1.lat_array,log1.lon_array,log2.lat_array,log2.lon_array,"Longitude","Latitude","NED vs. target Coordinates") +plotter.scatter_plot() diff --git a/gettingstarted/NED/ned_utilities.py b/gettingstarted/NED/ned_utilities.py new file mode 100644 index 00000000..cd0a53d9 --- /dev/null +++ b/gettingstarted/NED/ned_utilities.py @@ -0,0 +1,214 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +NED Utilities +""" +import math +from pymavlink import mavutil +import time +import numpy as np +import nvector as nv +from nvector import rad, deg +from math import sin, cos, atan2, radians, sqrt +from flight_plotter import Location + +wgs84 = nv.FrameE(name='WGS84') + + +class Nedvalues: + def __init__(self, north=0.0, east=0.0, down=0.0): + """ Create a new point at the origin """ + self.north = north + self.east = east + self.down = down + + def __repr__(self): + return 'Nedvalues({self.north}, {self.east}, {self.down})'.format(self=self) + + +class ned_controller: + + ################################################################################################ + # function: Get distance in meters + # parameters: Two global relative locations + # returns: Distance in meters + ################################################################################################ + def get_distance_meters(self, locationA, locationB): + # approximate radius of earth in km + R = 6373.0 + + lat1 = radians(locationA.lat) + lon1 = radians(locationA.lon) + lat2 = radians(locationB.lat) + lon2 = radians(locationB.lon) + + dlon = lon2 - lon1 + dlat = lat2 - lat1 + + a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2 + c = 2 * atan2(sqrt(a), sqrt(1 - a)) + + distance = (R * c) * 1000 + + # print("Distance (meters):", distance) + return distance + + # Sends velocity vector message to UAV vehicle + def send_ned_velocity(self, velocity_x, velocity_y, velocity_z, duration, vehicle): + """ + Move vehicle in a direction based on specified velocity vectors. + """ + msg = vehicle.message_factory.set_position_target_local_ned_encode( + 0, # time_boot_ms (not used) + 0, 0, # target system, target component + mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame + 0b0000111111000111, # type_mask (only speeds enabled) + 0, 0, 0, # x, y, z positions (not used) + velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s + 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) + 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) + + #for x in range(0, duration): + vehicle.send_mavlink(msg) + + time.sleep(0.1) + + # Sends velocity vector message to UAV vehicle + + def send_ned_stop(self, vehicle): + count = 1 + outerloopcounter = 1 + + currentVelocity = vehicle.velocity + north = currentVelocity[0] + south = currentVelocity[1] + msg = vehicle.message_factory.set_position_target_local_ned_encode( + 0, # time_boot_ms (not used) + 0, 0, # target system, target component + mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame + 0b0000111111000111, # type_mask (only speeds enabled) + 0, 0, 0, # x, y, z positions (not used) + -north*100,-south*100,0, # x, y, z velocity in m/s + 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) + 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) + + while True: + if vehicle.groundspeed > 1 and outerloopcounter <= 100: + if count == 100: + count = 1 + print(vehicle.groundspeed) + outerloopcounter = outerloopcounter + 1 + else: + count = count + 1 + + vehicle.send_mavlink(msg) + + if outerloopcounter == 100: + break + + msg = vehicle.message_factory.set_position_target_local_ned_encode( + 0, # time_boot_ms (not used) + 0, 0, # target system, target component + mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame + 0b0000111111000111, # type_mask (only speeds enabled) + 0, 0, 0, # x, y, z positions (not used) + 0,0, 0, # x, y, z velocity in m/s + 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) + 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) + + print ("MESSAGE HERE: ") + print (msg) + print ("++++++++++++++++++++++++++++++++++++++") + + time.sleep(0.1) + + # Sets NED given a current and target location + def setNed(self, current, target): + lat_C, lon_C = rad(current.lat), rad(current.lon) + lat_T, lon_T = rad(target.lat), rad(target.lon) + # create an n-vector for current and target + nvecC = nv.lat_lon2n_E(lat_C, lon_C) + nvecT = nv.lat_lon2n_E(lat_T, lon_T) + # create a p-vec from C to T in the Earth's frame + # the zeros are for the depth (depth = -1 * altitude) + p_CT_E = nv.n_EA_E_and_n_EB_E2p_AB_E(nvecC, nvecT, 0, 0) + # create a rotation matrix + # this rotates points from the NED frame to the Earth's frame + R_EN = nv.n_E2R_EN(nvecC) + # rotate p_CT_E so it lines up with current's NED frame + # we use the transpose so we can go from the Earth's frame to the NED frame + n, e, d = np.dot(R_EN.T, p_CT_E).ravel() + + return Nedvalues(n,e,d) + +def get_velocity(vehicle): + v = vehicle.velocity + return Nedvalues(v[0], v[1], v[2]) + +def rotate_velocity(ned_velocity, degrees_clockwise, preserve_down_component=True): + """ + Returns a Nedvalues object holding the velocity vector that is rotated + clockwise as seen from above. + + Parameters: + ned_velocity: a Nedvalues object representing a velocity. Must not be + Nedvalues(0.0, 0.0, 0.0), pure up or pure down. + degrees_clockwise: a float + preserve_down_component: a boolean + + When preserve_down_component is true, the down component of the velocity is + not changed. When it is false, the down component is changed, as the new velocity + is found by rotating around an axis that is orthogonal to ned_velocity but + as close to the down axis as possible. + + Here is the use case for this function: + A drone is traveling in any direction and at any speed. We want it to turn + at an X degree angle and send it a new NED. + + Example 0: + Our vehicle is flying and we want to turn 42 degrees clockwise (as seen from above). + + rotate_velocity(get_velocity(vehicle), 42.0) + + This would return a Nedvalues object. Its value would depend on the vehicle's + current velocity + + Example 1: + We have a velocity of north and 45 degrees up. We want to rotate this velocity + 180 degrees while not changing the down component. We can do so by calling: + + rotate_velocity(Nedvalues(3.0, 0.0, -3.0), 180.0) + + This would return Nedvalues(-3.0, 0.0, -3.0) + + Example 2: + We are flying north and up at 45 degrees. We want to rotate the velocity 180 + degrees while changing down component of velocity: + + rotate_velocity(Nedvalues(3.0, 0.0, -3.0), 180.0, False) + Would return: Nedvalues(-3.0, 0.0, 3.0) + + Example 3: + We are flying north west at 1.4 meters per second. We want to rotate 90 degrees + counter-clockwise: + + rotate_velocity(Nedvalues(1.0, -1.0, 0.0), -90.0) + would return Nedvalues(-1.0, -1.0, 0.0) + """ + # This uses Rodrigues' rotation formula. + # https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula + v = np.array([ned_velocity.north, ned_velocity.east, ned_velocity.down]) + theta = math.radians(degrees_clockwise) + down_axis = np.array([0.0, 0.0, 1.0]) + if preserve_down_component: + k = down_axis + else: + left = np.cross(v, down_axis) + k = np.cross(left, v) + magnitude = math.sqrt(np.dot(k, k)) + k = k * (1.0 / magnitude) + + # https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Statement + v_rot = v * math.cos(theta) + np.cross(k, v) * math.sin(theta) + k * (np.dot(k, v)) * (1.0 - math.cos(theta)) + return Nedvalues(v_rot[0], v_rot[1], v_rot[2]) diff --git a/gettingstarted/NED/simple_ned_example.py b/gettingstarted/NED/simple_ned_example.py new file mode 100644 index 00000000..c9516905 --- /dev/null +++ b/gettingstarted/NED/simple_ned_example.py @@ -0,0 +1,152 @@ +#!/ usr / bin / env python +# -*- coding: utf-8 -*- + +""" +Modified from 3DR simple_goto.py +Added code for flying using NED Velocity Vectors - Jane Cleland-Huang 1/15/18 +""" +import math +import os +import time +from math import sin, cos, atan2, radians, sqrt + +from dronekit import Vehicle, connect, VehicleMode, LocationGlobalRelative +from dronekit_sitl import SITL + +from flight_plotter import Location, CoordinateLogger, GraphPlotter +from ned_utilities import ned_controller + +################################################################################################ +# Standard Connect +################################################################################################ +def connect_virtual_vehicle(instance, home): + sitl = SITL() + sitl.download('copter', '3.3', verbose=True) + instance_arg = '-I%s' %(str(instance)) + home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2])) + sitl_args = [instance_arg, '--model', 'quad', home_arg] + sitl.launch(sitl_args, await_ready=True) + tcp, ip, port = sitl.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + print('Connecting to vehicle on: %s' % conn_string) + + vehicle = connect(conn_string) + vehicle.wait_ready(timeout=120) + print("Reached here") + return vehicle, sitl + + +################################################################################################ +# ARM and TAKEOFF +################################################################################################ + +# function: arm and takeoff +# parameters: target altitude (e.g., 10, 20) +# returns: n/a + +def arm_and_takeoff(aTargetAltitude): + """ + Arms vehicle and fly to aTargetAltitude. + """ + + print("Basic pre-arm checks") + # Don't try to arm until autopilot is ready + while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(1) + + print("home: " + str(vehicle.location.global_relative_frame.lat)) + + print("Arming motors") + # Copter should arm in GUIDED mode + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + + # Confirm vehicle armed before attempting to take off + while not vehicle.armed: + print(" Waiting for arming...") + time.sleep(1) + + print("Taking off!") + vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude + + while True: + # Break and return from function just below target altitude. + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * .95: + print("Reached target altitude") + break + time.sleep(1) + + +################################################################################################ +# function: Get distance in meters +# parameters: Two global relative locations +# returns: Distance in meters +################################################################################################ +def get_distance_meters(locationA, locationB): + # approximate radius of earth in km + R = 6373.0 + + lat1 = radians(locationA.lat) + lon1 = radians(locationA.lon) + lat2 = radians(locationB.lat) + lon2 = radians(locationB.lon) + + dlon = lon2 - lon1 + dlat = lat2 - lat1 + + a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2 + c = 2 * atan2(sqrt(a), sqrt(1 - a)) + + distance = (R * c) * 1000 + + # print("Distance (meters):", distance) + return distance + + +############################################################################################ +# Main functionality: Example of one NED command +############################################################################################ + +vehicle, sitl = connect_virtual_vehicle(1,([41.714436,-86.241713,0])) +arm_and_takeoff(10) +startingLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon) +targetLocation = LocationGlobalRelative(41.714500,-86.241600,0) +totalDistance = get_distance_meters(startingLocation,targetLocation) +print("Distance to travel " + str(totalDistance)) + +# Establish an instance of ned_controller +nedcontroller = ned_controller() + +# Get the NED velocity vector mavlink message +ned = nedcontroller.setNed(startingLocation, targetLocation) +nedcontroller.send_ned_velocity(ned.north, ned.east, ned.down, 1, vehicle) + +# Just wait for it to get there! +print("ARE WE THERE YET?") +currentLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon) +while get_distance_meters(currentLocation,targetLocation) > .05: + currentLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon) + distance_to_target = get_distance_meters(currentLocation, targetLocation) + print ('Distance: {0} Ground speed: {1} '.format(distance_to_target,vehicle.groundspeed)) + #print (distance_to_target) + +# How close did it get? +endLocation = Location(vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon) +print("Starting position: " + str(startingLocation.lat) + ", " + str(startingLocation.lon)) +print("Target position: " + str(targetLocation.lat) + ", " + str(targetLocation.lon)) +print("End position: " + str(endLocation.lat) + ", " + str(endLocation.lon)) + +print("Returning to Launch") +vehicle.mode = VehicleMode("RTL") + +# Close vehicle object before exiting script +print("Close vehicle object") +vehicle.close() + +# Shut down simulator if it was started. +if sitl is not None: + sitl.stop() + + diff --git a/gettingstarted/drone_model.py b/gettingstarted/drone_model.py new file mode 100644 index 00000000..8db59343 --- /dev/null +++ b/gettingstarted/drone_model.py @@ -0,0 +1,17 @@ +import json +class Drone_Model: + lat=0 + lon=0 + def __init__(self, id, lat, lon): + self.id = id + self.lat = lat + self.lon = lon + + def update_status(self, lat, lon): + self.lat = lat + self.lon = lon + + + def toJSON(self): + return json.dumps(self, default=lambda o: o.__dict__,sort_keys=True, indent=4) + \ No newline at end of file diff --git a/gettingstarted/hello_drone.py b/gettingstarted/hello_drone.py new file mode 100644 index 00000000..4a956c4f --- /dev/null +++ b/gettingstarted/hello_drone.py @@ -0,0 +1,40 @@ +# Import DroneKit-Python +from dronekit import connect, VehicleMode, time + +#Setup option parsing to get connection string +import argparse +parser = argparse.ArgumentParser(description='Print out vehicle state information') +parser.add_argument('--connect',help="vehicle connection target string.") +args=parser.parse_args() + +connection_string = args.connect +sitl = None + + +#Start SITL if no connection string specified +if not connection_string: + import dronekit_sitl + sitl = dronekit_sitl.start_default() + connection_string = sitl.connection_string() + +# Connect to the Vehicle. +# Set `wait_ready=True` to ensure default attributes are populated before `connect()` returns. +print "\nConnecting to vehicle on: %s" % connection_string +vehicle = connect(connection_string, wait_ready=True) + +#vehicle.wait_ready('autopilot_version') + +#Get some vehicle attributes (state) +print "Get some vehicle attribute values:" +print "GPS: %s" % vehicle.gps_0 +print "Battery: %s" % vehicle.battery +print "Last Heartbeat: %s" % vehicle.last_heartbeat +print "Is Armable?: %s" % vehicle.system_status.state +print "Mode: %s" % vehicle.mode.name # settable + +# Close vehicle object before exiting script +vehicle.close() + +time.sleep(5) + +print("Completed") \ No newline at end of file diff --git a/gettingstarted/multiple_drones/drone_model.py b/gettingstarted/multiple_drones/drone_model.py new file mode 100644 index 00000000..fb597cf8 --- /dev/null +++ b/gettingstarted/multiple_drones/drone_model.py @@ -0,0 +1,16 @@ +import json +class Drone_Model: + lat=0 + lon=0 + def __init__(self, id, lat, lon): + self.id = id + self.lat = lat + self.lon = lon + + def update_status(self, lat, lon): + self.lat = lat + self.lon = lon + + + def toJSON(self): + return json.dumps(self, default=lambda o: o.__dict__,sort_keys=True, indent=4) \ No newline at end of file diff --git a/gettingstarted/multiple_drones/multiple_drones1.py b/gettingstarted/multiple_drones/multiple_drones1.py new file mode 100644 index 00000000..686fb368 --- /dev/null +++ b/gettingstarted/multiple_drones/multiple_drones1.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +© Copyright 2015-2016, 3D Robotics. +simple_goto.py: GUIDED mode "simple goto" example (Copter Only) +Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto. +Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html +""" + +import time +from dronekit_sitl import SITL +from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative + + +from websocket import create_connection +from drone_model import Drone_Model + +ws = create_connection("ws://localhost:8000") + +# drone_count = 1 + +# drone_models={} + +def connect_virtual_vehicle(instance, home): + sitl = SITL() + sitl.download('copter', '3.3', verbose=True) + instance_arg = '-I%s' %(str(instance)) + home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2])) + sitl_args = [instance_arg, '--model', 'quad', home_arg] + sitl.launch(sitl_args, await_ready=True) + tcp, ip, port = sitl.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + print('Connecting to vehicle on: %s' % conn_string) + + vehicle = connect(conn_string) + vehicle.wait_ready(timeout=120) + print("Reached here") + + data_model = Drone_Model(instance+1,home[0],home[1]) + # drone_model[drone_count] = drone_model + + + + return vehicle, sitl, data_model + + +def custom_sleep(vehicle,drone_model, sleep_time): + current_time = 0 + while(current_time= aTargetAltitude * 0.95: + print("Reached target altitude") + break + # time.sleep(1) + custom_sleep(vehicle,data_model,1) + +def fly_drone(vehicle, data_model): + arm_and_takeoff(10, vehicle, data_model) + print("Set default/target airspeed to 3") + vehicle.airspeed = 3 + + print("Going towards first point for 30 seconds ...") + point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) + print (point1) + vehicle.simple_goto(point1) + + # sleep so we can see the change in map + #time.sleep(10) + custom_sleep(vehicle,data_model,10) + print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...") + point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) + vehicle.simple_goto(point2, groundspeed=10) + + # sleep so we can see the change in map + #time.sleep(10) + custom_sleep(vehicle,data_model,10) + + print("Returning to Launch") + vehicle.mode = VehicleMode("LAND") + print (vehicle.location.global_frame) + + # Close vehicle object before exiting script + print("Close vehicle object") + vehicle.close() + +vehicle, sitl, data_model_1 = connect_virtual_vehicle(0,([41.715446209367,-86.242847096132,0])) +vehicle2, sitl2, data_model_2 = connect_virtual_vehicle(1,([41.715469, -86.242543,0])) + +print("flying drone 1") +#arm_and_takeoff(10, vehicle, data_model_1) +fly_drone(vehicle, data_model_1) + +print("flying drone 2") +#arm_and_takeoff(10, vehicle2, data_model_2) +fly_drone(vehicle2, data_model_2) + + +# Shut down simulator if it was started. +if sitl: + sitl.stop() + sitl2.stop() diff --git a/gettingstarted/multiple_drones/multiple_drones2.py b/gettingstarted/multiple_drones/multiple_drones2.py new file mode 100644 index 00000000..a60871f5 --- /dev/null +++ b/gettingstarted/multiple_drones/multiple_drones2.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +© Copyright 2015-2016, 3D Robotics. +simple_goto.py: GUIDED mode "simple goto" example (Copter Only) +Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto. +Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html +""" + +#from __future__ import print_function +import time +from dronekit_sitl import SITL +from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative + +def connect_virtual_vehicle(instance, home): + sitlx = SITL() + sitlx.download('copter', '3.3', verbose=True) + instance_arg = '-I%s' %(str(instance)) + print("Drone instance is: %s" % instance_arg) + home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2])) + sitl_args = [instance_arg, '--model', 'quad', home_arg] + sitlx.launch(sitl_args, await_ready=True) + tcp, ip, port = sitlx.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + print('Connecting to vehicle on: %s' % conn_string) + + vehicle = connect(conn_string) + vehicle.wait_ready(timeout=120) + print("Reached here") + return vehicle, sitlx + +def arm_and_takeoff(aTargetAltitude): + """ + Arms vehicle and fly to aTargetAltitude. + """ + print("Basic pre-arm checks") + # Don't try to arm until autopilot is ready + while not (vehicle.is_armable and vehicle2.is_armable): + if (not vehicle.is_armable): + print(" Waiting for vehicle 1 to initialise...") + if (not vehicle2.is_armable): + print(" Waiting for vehicle 2 to initialise...") + time.sleep(3) + + print("Arming motors") + vehicle.mode = VehicleMode("GUIDED") + vehicle.armed = True + vehicle2.mode = VehicleMode("GUIDED") + vehicle2.armed = True + + while not (vehicle.armed and vehicle2.armed): + print(" Waiting for arming...") + time.sleep(1) + + print("Vehicle armed!") + print("Both drones are now Taking off!") + vehicle.simple_takeoff(aTargetAltitude) # Take off to target altitude + vehicle2.simple_takeoff(aTargetAltitude) + + # Wait for both drones + while True: + print(" Altitude V1: ", vehicle.location.global_relative_frame.alt) + print(" Altitude V2: ", vehicle2.location.global_relative_frame.alt) + + # Break and return from function just below target altitude. + if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95 and vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: + print("Both Drones Reached target altitude") + break + time.sleep(1) + +vehicle, sitl = connect_virtual_vehicle(0,([41.715446209367,-86.242847096132,0])) +vehicle2, sitl2 = connect_virtual_vehicle(1,([41.715469, -86.242543,0])) + +arm_and_takeoff(10) + +# Shut down simulator if it was started. +if sitl: + sitl.stop() + sitl2.stop() diff --git a/gettingstarted/multiple_drones/multiple_drones3.py b/gettingstarted/multiple_drones/multiple_drones3.py new file mode 100644 index 00000000..2c1264bb --- /dev/null +++ b/gettingstarted/multiple_drones/multiple_drones3.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +© Copyright 2015-2016, 3D Robotics. +simple_goto.py: GUIDED mode "simple goto" example (Copter Only) +Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto. +Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html +""" + +import time +from dronekit_sitl import SITL +from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative + + + +copters = [] +sitls = [] +drone_models = [] + + +def connect_virtual_vehicle(instance, home): + sitl = SITL() + sitl.download('copter', '3.3', verbose=True) + instance_arg = '-I%s' %(str(instance)) + print("Drone instance is: %s" % instance_arg) + home_arg = '--home=%s, %s,%s,180' % (str(home[0]), str(home[1]), str(home[2])) + sitl_args = [instance_arg, '--model', 'quad', home_arg] + sitl.launch(sitl_args, await_ready=True) + tcp, ip, port = sitl.connection_string().split(':') + port = str(int(port) + instance * 10) + conn_string = ':'.join([tcp, ip, port]) + print('Connecting to vehicle on: %s' % conn_string) + + vehicle = connect(conn_string) + vehicle.wait_ready(timeout=120) + + # Collections + copters.append(vehicle) + sitls.append(sitl) + +def copters_at_altitude(aTargetAltitude): + while True: + at_altitude = True + ctr=1 + for c in copters: + print ('Copter ID: {} at altitude {} '.format(ctr,str(c.location.global_relative_frame.alt))) + ctr = ctr + 1 + if (not c.location.global_relative_frame.alt >= aTargetAltitude * 0.95): + at_altitude = False + time.sleep(3) + + if at_altitude == True: + print("All drones have reached their target altitudes") + break + +def copters_arm(): + for c in copters: + c.mode = VehicleMode("GUIDED") + c.armed = True + + for c in copters: + while not (c.armed): + time.sleep(1) + +def land_drones(): + for c in copters: + c.mode = VehicleMode("LAND") + print ("LANDING....") + time.sleep(30) + +def copters_armable(): + + while True: + unarmable = False + for c in copters: + if (not c.is_armable): + unarmable = True + time.sleep(3) + + if unarmable == False: + break + + +def arm_and_takeoff(aTargetAltitude): + """ + Arms vehicle and fly to aTargetAltitude. + """ + print("Basic pre-arm checks") + # Don't try to arm until autopilot is ready + copters_armable() + + print("Arming motors") + copters_arm() + + print("Vehicle armed!") + + print("All drones are now Taking off!") + aTargetAltitude = 10 + for c in copters: + c.simple_takeoff(aTargetAltitude) # Take off to target altitude + + print("Waiting for copters to ascend") + copters_at_altitude(aTargetAltitude) + +# Starting coordinates +coordinates = [41.714469, -86.241786,0] + +# Copter list +for n in range(5): + coordinates = [coordinates[0],coordinates[1]-(0.00005*n),coordinates[2]] + connect_virtual_vehicle(n,coordinates) + +# Arm and takeoff to 10 meters +arm_and_takeoff(10) + +# Land them +land_drones() + +# Close all vehicles +for c in copters: + c.close() + +# Shut down simulators +for s in sitls: + s.stop() + diff --git a/gettingstarted/run_sitl.sh b/gettingstarted/run_sitl.sh new file mode 100644 index 00000000..c8f8ec53 --- /dev/null +++ b/gettingstarted/run_sitl.sh @@ -0,0 +1,10 @@ +#cd "$(dirname "$(realpath "$0")")" +REPO=/home/uav/git/ardupilot/ArduCopter + +pkill -9 python + +gnome-terminal -x bash -c "dronekit-sitl copter --home=41.71480,-86.24187" +gnome-terminal --working-directory=$REPO -x bash -c "python3 ../Tools/autotest/sim_vehicle.py -j4 --map --console" + + + diff --git a/gettingstarted/server.py b/gettingstarted/server.py new file mode 100644 index 00000000..f639b2b0 --- /dev/null +++ b/gettingstarted/server.py @@ -0,0 +1,33 @@ +from simple_websocket_server import WebSocketServer, WebSocket + + +class WebServer(WebSocket): + def handle(self): + for client in clients: + if client != self: + client.send_message(self.data) + + def connected(self): + print(self.address, 'connected') + for client in clients: + client.send_message(self.address[0] + u' - connected') + clients.append(self) + + def handle_close(self): + clients.remove(self) + print(self.address, 'closed') + for client in clients: + client.send_message(self.address[0] + u' - disconnected') + + # def send(self, data): + # for client in clients: + # if client !=self: + # client.send_message(data) + + + + +clients = [] + +server = WebSocketServer('', 8000, WebServer) +server.serve_forever() \ No newline at end of file diff --git a/gettingstarted/setup_dronekit.sh b/gettingstarted/setup_dronekit.sh new file mode 100644 index 00000000..3c8abd66 --- /dev/null +++ b/gettingstarted/setup_dronekit.sh @@ -0,0 +1,62 @@ +#! /bin/bash +################################ MODIFY REPO PARAMETERS HERE ############################## +# Clone the Dronology Repository +# modify these two pointers if you want to change where the repos should be cloned +# defaults to $HOME/git (e.g., /home/bayley/git) +# will fail and/or result in unexpected behavior if parent directory does not exist or is not an absolute path! +export REPOS_PARENT_DIR=$HOME +export REPOS_DIR=git +################################ MODIFY REPO PARAMETERS HERE ############################## + +# prereqs +export CURR_VER=`lsb_release -rs` +function version_ge() { test "$(echo "$@" | tr " " "\n" | sort -rV | head -n 1)" == "$1"; } + +# install python +sudo apt-get install software-properties-common +sudo add-apt-repository ppa:deadsnakes/ppa +sudo apt-get update +sudo apt-get install python2.7 +# install pip +sudo apt-get install python-pip +# install git +sudo apt-get install git + + +cd $REPOS_PARENT_DIR +# makes the directory if it doesn't already exist +mkdir $REPOS_DIR +cd $REPOS_DIR + +# Clone the ArduPilot repository. +git clone git://github.com/ArduPilot/ardupilot.git +cd ardupilot +git submodule update --init --recursive + +# Install ardupilot dependencies +#if version_ge $CURR_VER '16.0'; then +export WXGTK_VERSION=3.0 +#else +#export WXGTK_VERSION=2.8 +#fi +echo "Linux Version is: $CURR_VER -- wxgtk $WXGTK_VERSION needs to be installed" +sudo apt-get install python-dev python-opencv python-wxgtk$WXGTK_VERSION python-pip python-matplotlib python-pygame python-lxml +sudo pip install future pymavlink MAVProxy + +sudo pip install pyudev +sudo pip install PyYAML +sudo pip install nvector +sudo pip install geographiclib +sudo pip install numpy==1.16.6 +sudo pip install boltons +sudo pip install dronekit==2.9.1 +sudo pip install dronekit-sitl==3.2.0 + + +# Build SITL +export PATH=$PATH:$REPOS_PARENT_DIR/$REPOS_DIR/ardupilot/Tools/autotest +export PATH=/usr/lib/ccache:$PATH +cd ArduCopter +sim_vehicle.py -w -j4 --map + + diff --git a/gettingstarted/simple_connect_DRONE.py b/gettingstarted/simple_connect_DRONE.py new file mode 100644 index 00000000..dab1bba5 --- /dev/null +++ b/gettingstarted/simple_connect_DRONE.py @@ -0,0 +1,14 @@ +from dronekit import connect, VehicleMode, time +import dronekit +import os + + +vehicle = dronekit.connect("/dev/ttyUSB0",wait_ready=True) +vehicle.wait_ready(timeout=120) + +while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(3) +print("Arming motors") +vehicle.mode = VehicleMode("GUIDED") +vehicle.armed = True \ No newline at end of file diff --git a/gettingstarted/simple_connect_SITL.py b/gettingstarted/simple_connect_SITL.py new file mode 100644 index 00000000..ace55fed --- /dev/null +++ b/gettingstarted/simple_connect_SITL.py @@ -0,0 +1,25 @@ +from dronekit import connect, VehicleMode, time +import dronekit_sitl +import dronekit +import os + +ardupath ="/win/Work/git/ardupilot" +home="41.715446209367,-86.242847096132" +sitl_defaults = os.path.join(ardupath, 'Tools', 'autotest', 'default_params', 'copter.parm') +sitl_args = ['-I{}'.format(0), '--home', home, '--model', '+', '--defaults', sitl_defaults] +sitl = dronekit_sitl.SITL(path=os.path.join(ardupath, 'build', 'sitl', 'bin', 'arducopter')) +sitl.launch(sitl_args, await_ready=True) + +tcp, ip, port = sitl.connection_string().split(':') +port = str(int(port) + 0 * 10) +conn_string = ':'.join([tcp, ip, port]) + +vehicle = dronekit.connect(conn_string) +vehicle.wait_ready(timeout=120) + +while not vehicle.is_armable: + print(" Waiting for vehicle to initialise...") + time.sleep(3) +print("Arming motors") +vehicle.mode = VehicleMode("GUIDED") +vehicle.armed = True \ No newline at end of file diff --git a/gettingstarted/simple_goto.py b/gettingstarted/simple_goto.py new file mode 100644 index 00000000..2b63b4b1 --- /dev/null +++ b/gettingstarted/simple_goto.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +""" +© Copyright 2015-2016, 3D Robotics. +simple_goto.py: GUIDED mode "simple goto" example (Copter Only) +Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto. +Full documentation is provided at http://python.dronekit.io/examples/simple_goto.html +""" + +from __future__ import print_function +import time +from dronekit_sitl import SITL +from dronekit import Vehicle, VehicleMode, connect, LocationGlobalRelative + + +import json +from websocket import create_connection +from drone_model import Drone_Model +ws = create_connection("ws://localhost:8000") + +drone_model_object = Drone_Model(1,0,0) + +# Set up option parsing to get connection string +import argparse +parser = argparse.ArgumentParser(description='Commands vehicle using vehicle.simple_goto.') +parser.add_argument('--connect', + help="Vehicle connection target string. If not specified, SITL automatically started and used.") +args = parser.parse_args() +connection_string = args.connect +sitl = None + + +# Start SITL if no connection string specified +# This technique for starting SITL allows us to specify defffaults +if not connection_string: + sitl_defaults = '~/git/ardupilot/tools/autotest/default_params/copter.parm' + sitl = SITL() + + sitl.download('copter', '3.3', verbose=True) + sitl_args = ['-I0', '--model', 'quad', '--home=35.361350, 149.165210,0,180'] + sitl.launch(sitl_args, await_ready=True, restart=True) + connection_string = 'tcp:127.0.0.1:5760' + +# Connect to the Vehicle +print('Connecting to vehicle on: %s' % connection_string) +vehicle = connect(connection_string, wait_ready=True, baud=57600) +print ('Current position of vehicle is: %s' % vehicle.location.global_frame) + + +def custom_sleep(drone_model, sleep_time): + current_time = 0 + while(current_time= aTargetAltitude * 0.95: + print("Reached target altitude") + break + time.sleep(1) + + + + +arm_and_takeoff(10) + + + + +print("Set default/target airspeed to 3") +vehicle.airspeed = 3 + +print("Going towards first point for 30 seconds ...") +point1 = LocationGlobalRelative(-35.361354, 149.165218, 20) +vehicle.simple_goto(point1) + + + + +# sleep so we can see the change in map +#time.sleep(30) +custom_sleep(drone_model_object,30) + + +print("Going towards second point for 30 seconds (groundspeed set to 10 m/s) ...") +point2 = LocationGlobalRelative(-35.363244, 149.168801, 20) +vehicle.simple_goto(point2, groundspeed=10) + +# sleep so we can see the change in map +#time.sleep(30) +custom_sleep(drone_model_object,30) + +print("Returning to Launch") +vehicle.mode = VehicleMode("RTL") + +# Close vehicle object before exiting script +print("Close vehicle object") +vehicle.close() + +# Shut down simulator if it was started. +if sitl: + sitl.stop() + + + + + + + + + + + diff --git a/gettingstarted/utility.py b/gettingstarted/utility.py new file mode 100644 index 00000000..9d3ee539 --- /dev/null +++ b/gettingstarted/utility.py @@ -0,0 +1,11 @@ +class Utility: + + def __init__(self): + pass + def custom_sleep(self, vehicle,drone_model, sleep_time): + current_time = 0 + while(current_time + + + + + + + + + +
+ + + + + diff --git a/gettingstarted/webApp/map.js b/gettingstarted/webApp/map.js new file mode 100644 index 00000000..df39e948 --- /dev/null +++ b/gettingstarted/webApp/map.js @@ -0,0 +1,95 @@ +marker_count=0; +markers = {} +number_drones = 2 +function initMap() { + + // map center on the screen - 50,50 + // change it according to the home location of the drone. + var myLatLng = new google.maps.LatLng( 41.714469, -86.241786 ) + myOptions = { + zoom: 4, + center: myLatLng, + mapTypeId: google.maps.MapTypeId.ROADMAP + } + map = new google.maps.Map( document.getElementById( 'map-canvas' ), myOptions ) + + add_markers(myLatLng, map) + + + + + //markers[1] = marker1; + + //marker2.setMap(map); + //marker3.setMap(map); + +} +function get_marker_for_drone(drone_id){ + return markers[drone_id] + +} +function add_markers(position,map){ + while(marker_count!=number_drones){ + marker_count+=1; + marker = new google.maps.Marker( {position: position, map: map} ); + marker.setMap( map ); + markers[marker_count] = marker; + + } + console.log(markers); +} + +function move(id,lat,lon) { + console.log("moving", id, lat, lon); + marker_obj = get_marker_for_drone(id) + console.log(marker_obj); + marker_obj.setPosition( new google.maps.LatLng( lat, lon ) ); + + + // map.panTo( new google.maps.LatLng( 0, 0 ) ); + +}; + +initMap(); +doConnect(); + + + +function doConnect() +{ + websocket = new WebSocket('ws://localhost:8000/'); + websocket.onopen = function(evt) { onOpen(evt) }; + websocket.onclose = function(evt) { onClose(evt) }; + websocket.onmessage = function(evt) { onMessage(evt) }; + websocket.onerror = function(evt) { onError(evt) }; +} + +function onOpen(evt) +{ +console.log("connected\n"); + +} + +function onClose(evt) +{ +console.log("disconnected\n"); + +} + +function onMessage(evt) +{ + drone = JSON.parse(evt.data); + + console.log(drone+'\n'); + move(drone.id, drone.lat,drone.lon) + +} + +function onError(evt) +{ + console.log('error: ' + evt.data + '\n'); + +websocket.close(); + +} +