import dronekit
from dronekit import connect, VehicleMode, LocationGlobalRelative
import dronekit_sitl
import time
import argparse
import math
import sys
import threading
import serial
from serial.tools.list_ports import comports
from serial.tools import hexlify_codec
from pymavlink import mavutil
db=[]
drone_sysids=[]
numberofsattelitesarray=[]
ports=[]
nn=[]
vehicleobjects=[]
vehicle_yaw=[]
distance=[]
def ask_for_port():
sys.stderr.write('\n--- Available ports:\n')
for n, (port, desc, hwid) in enumerate(sorted(comports()), 1):
sys.stderr.write('--- {:2}: {}\n'.format(n, port))
print(type(port))
ports.append(port)
def connect_drones(num_drones):
ask_for_port()
for i in range(num_drones):
vehicle = connect(ports[i],baud=57600,wait_ready=True,timeout=300,heartbeat_timeout=300)
sattelitecount=vehicle.gps_0.satellites_visible;
numberofsattelitesarray.append(sattelitecount)
sysid=int(vehicle.parameters['SYSID_THISMAV'])
vehicleobjects.append(vehicle);
print("gps sattelites:",numberofsattelitesarray)
print(vehicleobjects)
distance=[]
def distance_between_drones():
for i in range(1,len(vehicleobjects)):
v1 = vehicleobjects[0].location.global_relative_frame
v2=vehicleobjects[i].location.global_relative_frame
distance.append(distance_between_points(v1,v2))
print(distance)
def check_distance():
for i in range(len(distance)-1):
if(distance[i+1]-distance[i]<4):
print(distance[i+1]-distance[i])
return False
return True
# Function to get distance between two points
def distance_between_points(point1, point2):
lat_difference = point2.lat - point1.lat
lon_difference = point2.lon - point1.lon
return math.sqrt((lat_difference * lat_difference) + (lon_difference * lon_difference)) * 1.113195e5
def arm_and_takeoff(i, altitude):
if(check_satellites()==5 and check_distance()):
while not vehicleobjects[i].is_armable:
print(" Waiting for vehicle to initialise...")
time.sleep(2)
print("Arming motors")
vehicleobjects[i].mode = VehicleMode("GUIDED")
vehicleobjects[i].armed=True
while not vehicleobjects[i].armed:
print(" Waiting for arming...")
time.sleep(3)
print(vehicleobjects[i],i)
vehicleobjects[i].airspeed = 5
vehicleobjects[i].simple_takeoff(altitude)
else:
arm_and_takeoff(i,altitude)
time.sleep(1)
def land():
for i in range(len(vehicleobjects)):
vehicleobjects[i].mode = VehicleMode("RTL")
print("Land")
def close(): #this function will close vehicle object and shut down the simulator
for drone in vehicleobjects:
drone.close()
print("Closed")
def check_satellites():
noos=0
for i in range(len(numberofsattelitesarray)):
if numberofsattelitesarray[i]>=13:
noos=noos+1
return noos
connect_drones(5)
print("connection:",vehicleobjects)
for i in range(5):
arm_and_takeoff(i,5)
for i in range(5):
while True:
print(" Altitude: ", vehicleobjects[i].location.global_relative_frame.alt)
# Break and return from function just below target altitude.
if vehicleobjects[i].location.global_relative_frame.alt >= 5 * 0.95:
print("Reached target altitude")
break
time.sleep(1)
print("Formation2")
# Move the drones to a new location.
# formstion2
# display details
formation2_alt=[5,6,7,6,5]
i=0
for vehicle in vehicleobjects:
vehicle.simple_goto(LocationGlobalRelative(vehicle.location.global_relative_frame.lat,vehicle.location.global_relative_frame.lon,formation2_alt[i]))
i=i+1
i=0
for vehicle in vehicleobjects:
while True:
print(" Altitude: ", vehicle.location.global_relative_frame.alt)
# Break and return from function just below target altitude.
if vehicle.location.global_relative_frame.alt >= formation2_alt[i] * 0.95:
print("Reached target altitude")
break
time.sleep(1)
i=i+1
print("Formation3")
# # formstion3
formation3_alt=[7,6,5,6,7]
i=0
for vehicle in vehicleobjects:
vehicle.simple_goto(LocationGlobalRelative(vehicle.location.global_relative_frame.lat,vehicle.location.global_relative_frame.lon,formation3_alt[i]))
i=i+1
i=0
for vehicle in vehicleobjects:
while True:
print(" Altitude: ", vehicle.location.global_relative_frame.alt)
# Break and return from function just below target altitude.
if vehicle.location.global_relative_frame.alt >= formation3_alt[i] * 0.95:
print("Reached target altitude")
break
time.sleep(1)
i=i+1
print("Formation4")
# formation4
formation4_alt=[9,6.6,4.9,3.7,3.32]
i=0
for vehicle in vehicleobjects:
vehicle.simple_goto(LocationGlobalRelative(vehicle.location.global_relative_frame.lat,vehicle.location.global_relative_frame.lon,formation4_alt[i]))
i=i+1
i=0
for vehicle in vehicleobjects:
while True:
print(" Altitude: ", vehicle.location.global_relative_frame.alt)
# Break and return from function just below target altitude.
if vehicle.location.global_relative_frame.alt >= formation4_alt[i] * 0.95:
print("Reached target altitude")
break
time.sleep(1)
i=i+1
print("Formation 5")
# # formstion5
formation5_alt=[5,5,5,5,5]
i=0
for vehicle in vehicleobjects:
vehicle.simple_goto(LocationGlobalRelative(vehicle.location.global_relative_frame.lat,vehicle.location.global_relative_frame.lon,formation5_alt[i]))
i=i+1
i=0
for vehicle in vehicleobjects:
while True:
print(" Altitude: ", vehicle.location.global_relative_frame.alt)
# Break and return from function just below target altitude.
if vehicle.location.global_relative_frame.alt >= formation5_alt[i] * 0.95:
print("Reached target altitude")
break
time.sleep(1)
i=i+1
# # Land the drones.
land()
close()
I am getting dronekit.TimeoutError: wait_ready experienced a timeout after 30 seconds and WARNING: dronekit: Link timeout, no heartbeat in last 5 seconds ERROR When trying to connect 5 drones using com connection using telemetry.I am using dronekit.