# -*-coding:utf8 -*-
from dronekit import connect, VehicleMode, LocationGlobalRelative, APIException
import time
#import exceptions
import math
import argparse
import serial #串口
import threading
opponent_state = 0
this_vehicle_state = 0
def re_message(): #接收消息
global opponent_state
while 1:
data=s.readline() #一次接收1024字节1 s.recv s为和数传进行连接
postion=data.decode("utf8",'ignore')
postion.strip() #方法用于移除字符串头尾指定的字符(默认为空格或换行符)
b=postion.split(' ') #分隔
#打印信息
opponent_state=int(b[0])#1号机的this_vehicle_state
opponent_gps=b[1]
opponent_velocity=b[2]
print ("Zhao vehicle state: %s" % opponent_state)
print ("Zhao vehicle GPS: %s" % opponent_gps)
print ("Zhao vehicle velocity: %s" % opponent_velocity)
def send_message():
global this_vehicle_state
while 1:
time.sleep(1)
print ("B battery: %s" % vehicle.battery)
print("B vehicle state: %s" % this_vehicle_state)
global_frame=vehicle.location.global_frame
velocity=vehicle.velocity
print("B vehicle state: %s" % vehicle.velocity)
data=str(this_vehicle_state) + " " + str(global_frame) + " " + str(velocity) + "\n"
s.write(data.encode()) #写进s中
def arm_and_takeoff(targetHeight): #飞机初始化起飞
#1、飞机自检
while vehicle.is_armable!=True:
print("Waiting for vehicle to become aramable")
time.sleep(1)
print("Vehicle is now armable")
#2、确定飞机模式
vehicle.mode = VehicleMode("GUIDED")
while vehicle.mode!="GUIDED":
print("Waiting for drone to enter GUIDED flight mode")
time.sleep(1)
print("Vehicle now in GUIDED MODE. Have fun!!")
#3、飞机解锁
vehicle.armed = True
# while vehicle.armed==False:
# print("Waiting for drone to become armed")
time.sleep(3)
# print("Look out! Virtual props are spinning!")
#4、飞行
vehicle.simple_takeoff(targetHeight) ##meters
#5、打印信息
while True:
print("Current Altitude:", vehicle.location.global_relative_frame.alt)
if vehicle.location.global_relative_frame.alt>=.95*targetHeight:
break
time.sleep(1)
print("Target altitude reached!!")
return None
def get_distance_meters(targetLocation, currentLocation):#获取定点距离
dLat = targetLocation.lat - currentLocation.lat
dLon = targetLocation.lon - currentLocation.lon
return math.sqrt((dLon*dLon)+(dLat*dLat))*1.113195e5
def goto(targetLocation):
#到定点
distanceToTargetLocation = get_distance_meters(targetLocation, vehicle.location.global_relative_frame)
vehicle.simple_goto(targetLocation)
while vehicle.mode.name == "GUIDED":
currentDistance = get_distance_meters(targetLocation,vehicle.location.global_relative_frame)
if currentDistance < distanceToTargetLocation*.01:
print("Reached target waypoint")
time.sleep(2)
break
time.sleep(1)
return None
if __name__=='__main__':
#1、连接
vehicle = connect('/dev/ttyACM0', wait_ready=True, baud=57600) #jetson通过串口与飞控连接
print("connect ...... ..... .... ")
print("connect ...... ..... .... ")
print("connect ...... ..... .... ")
s=serial.Serial('/dev/ttyTHS0',115200,8,'N',1) #jetson通过串口和数传连接
print("connect the ttl ...... .......")
print("connect the ttl ...... .......")
print("connect the ttl ...... ......")
#2、发送接收线程
re = threading.Thread(target=re_message)
re.start()
se = threading.Thread(target=send_message)
se.start()
#3、设置点位
wp = LocationGlobalRelative(32.6874439, 119.5353102, 10) #创建目标位置 纬度 经度 海拔
while 1:
if opponent_state == 1 : #第一台飞机发指令
arm_and_takeoff(10) #起飞
goto(wp) #到定点
#2号到达指定点后this_vehicle_state=1 给1号发送到opponent_state,1号机降落
this_vehicle_state = 1
vehicle.mode = VehicleMode("LAND")#降落
print("LAND mode send to ardupilot........")
while vehicle.mode != 'LAND':
print("Waiting for drone to enter LAND mode")
time.sleep(1)
print("Vehicle in LAND mode")
break
#while 1:
# if opponent_high > 8 :
# arm_and_takeoff(10)
# if vehicle.location.global_relative_frame.alt > 8 :
# if opponent_high < 4 :
# vehicle.mode = VehicleMode("LAND")
# while vehicle.mode != 'LAND':
# print("Waiting for drone to enter LAND mode")
# time.sleep(1)
## print("Vehicle in LAND mode")
from dronekit import connect, VehicleMode, LocationGlobalRelative, APIException
import time
import socket
import math
import argparse
import serial
import threading
opponent_state = 0
this_vehicle_state = 0
def re_message():
global opponent_state
while True:
data=s.readline()
postion=data.decode("utf8",'ignore')
postion.strip()
b=postion.split(' ')
opponent_state=int(b[0])#2号机的this_vehicle_state
opponent_gps=b[1]
opponent_velocity=b[2]
print ("B vehicle state: %s" % opponent_state)
print ("B vehicle GPS: %s" % opponent_gps)
print ("B vehicle velocity: %s" % opponent_velocity)
def send_message():
global this_vehicle_state
while True:
time.sleep(1)
print("Zhao Local Location: %s" % vehicle.location.local_frame)
print("Zhao Attitude: %s" % vehicle.attitude)
print("Zhao Battery: %s" % vehicle.battery)
print("Zhao vehicle state: %s" % this_vehicle_state)
global_frame=vehicle.location.global_frame
velocity=vehicle.velocity
data=str(this_vehicle_state) + " " + str(global_frame) + " " + str(velocity) + "\n"
s.write(data.encode())
def arm_and_takeoff(targetHeight):
while vehicle.is_armable!=True:
print("Waiting for vehicle to become aramable")
time.sleep(1)
print("Vehicle is now armable")
vehicle.mode = VehicleMode("GUIDED")
while vehicle.mode!="GUIDED":
print("Waiting for drone to enter GUIDED flight mode")
time.sleep(1)
print("Vehicle now in GUIDED MODE. Have fun!!")
vehicle.armed = True
# while vehicle.armed==False:
# print("Waiting for drone to become armed")
time.sleep(3)
# print("Look out! Virtual props are spinning!")
vehicle.simple_takeoff(targetHeight) ##meters
while True:
print("Current Altitude:", vehicle.location.global_relative_frame.alt)
if vehicle.location.global_relative_frame.alt>=.95*targetHeight:
break
time.sleep(1)
print("Target altitude reached!!")
return None
def get_distance_meters(targetLocation, currentLocation):
dLat = targetLocation.lat - currentLocation.lat
dLon = targetLocation.lon - currentLocation.lon
return math.sqrt((dLon*dLon)+(dLat*dLat))*1.113195e5
def goto(targetLocation):
distanceToTargetLocation = get_distance_meters(targetLocation, vehicle.location.global_relative_frame)
vehicle.simple_goto(targetLocation)
while vehicle.mode.name == "GUIDED":
currentDistance = get_distance_meters(targetLocation,vehicle.location.global_relative_frame)
if currentDistance < distanceToTargetLocation*.01:
print("Reached target waypoint")
time.sleep(2)
break
time.sleep(1)
return None
if __name__=='__main__':
#1、连接飞控
vehicle = connect('/dev/ttyACM0', wait_ready=True, baud=57600)
print("connect ardupilot..... .... ")
print("connect ardupilot..... .... ")
print("connect ardupilot..... .... ")
#2、连接数传
s = serial.Serial('/dev/ttyTHS0',115200,8,'N',1)
print("connect the ttl..... .... ")
print("connect the ttl..... .... ")
print("connect the ttl..... .... ")
re = threading.Thread(target=re_message)
re.start()
se = threading.Thread(target=send_message)
se.start()
wp = LocationGlobalRelative(32.6874453, 119.5349604, 12)
arm_and_takeoff(12)
goto(wp)
#到达指定点后this_vehicle_state=1 给2号发送到opponent_state,2号接收到后起飞
this_vehicle_state = 1
while 1:
#2号机到达后传过来的指令
if opponent_state == 1 :
vehicle.mode = VehicleMode("LAND")
while vehicle.mode != 'LAND':
print("Waiting for drone to enter LAND mode")
time.sleep(1)
print("Vehicle in LAND mode")
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)