无人机编程donekit及通讯（二）

后起飞无人机

1、Jetson连接飞控

vehicle = connect('/dev/ttyACM0', wait_ready=True, baud=921600)

串口连接，波频是921600

2、socket网络通信

s=socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
s.bind(("192.168.1.102",8080)) #绑定服务器的ip和端口 
print("connect ......     .....            ....     ")
print("connect ......     .....            ....     ")
print("connect ......     .....            ....     ")

Socket通信原理:

负责进程之间的网络数据传输，好比数据的搬运工。
服务器端先初始化Socket，然后与端口绑定(bind)，对端口进行监听(listen)，调用accept阻塞，等待客户端连接。在这时如果有个客户端初始化一个Socket，然后连接服务器(connect)，如果连接成功，这时客户端与服务器端的连接就建立了。客户端发送数据请求，服务器端接收请求并处理请求，然后把回应数据发送给客户端，客户端读取数据，最后关闭连接，一次交互结束。

类似于打电话：
A给B打电话，拨号等待接听，B接听，OK开始通信

它是网络通信过程中端点的抽象表示，包含进行网络通信必须的五种信息：

• 连接使用的协议

• 本地主机的IP地址

• 本地进程的协议端口

• 远地主机的IP地址

• 远地进程的协议端口

套接字(服务器):

在任何类型的通信开始之前，网络应用程序都必须创建套接字。

有两种类型的套接字：基于文件的和面向网络的。

基于文件AF_UNIX

面向网络的AF_INET

套接字地址（IP口端）：主机-端口对

做个比喻，套接字就像一个电话插孔，主机名和端口号就像区号和号码。
当程序之间需要通信时，需要知道对端的主机名（IP）和端口号。

• 面向连接的套接字

TCP套接字的名字SOCK_STREAM

• 无连接的套接字

UDP套接字的名字SOCK_DGRAM

3、多线程和多进程

re = threading.Thread(target=re_message)     #创建一个接收信息的进程
re.start()
se = threading.Thread(target=send_message)    #创建一个发送信息的进程
se.start()

Python 实现多线程编程需要借助于 threading 模块

threading 模块中最核心的内容是 Thread 这个类。

我们要创建 Thread 对象,然后让它们运行，每个 Thread 对象代表一个线程，在每个线程中我们可以让程序处理不同的任务，这就是多线程编程。

• 接受另一无人机信息

def re_message():
    global opponent_high
    while 1:
        time.sleep(1)                           #每一秒发一次
        #client是通过 sk.recv(1024)来进行接受数据，而1024表示，最多每次接受1024字节
        data=s.recv(1024)                       
        postion=data.decode("utf8",'ignore')    #解码忽略错误提示
        b=postion.split(' ')                    #把传输信息分离
        opponent_high=float(b[0])
        opponent_gps=b[1]
        opponent_velocity=b[2]
        print ("Opponent vehicle height: %s" % opponent_high)
        print ("Opponent vehicle GPS: %s" % opponent_gps)
        print ("Opponent vehicle velocity: %s" % opponent_velocity)

•  飞控发送给Jetson信息

def send_message():                                       #发送给Jetson的信息
    while 1:
        time.sleep(1)
        print (" Battery: %s" % vehicle.battery)
        alt=vehicle.location.global_relative_frame.alt
        print("This vehicle height: %s" % alt)
        global_frame=vehicle.location.global_frame
        print("This vehicle GPS: %s" % global_frame)
        velocity=vehicle.velocity
        print("This vehicle velocity: %s" % velocity)
        data=str(alt) + " " + str(global_frame) + " " + str(velocity)
        s.sendto(data.encode(),("192.168.1.101",8080))  #飞控编码发给Jetson

4、飞行协同逻辑

arm_and_takeoff(10)
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")

 

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.location.global_relative_frame.alt为相对于home点的高度
# simple_goto函数将位置发送给无人机，生成一个目标航点 
    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