Dronekit代码学习（五）基于Gps下的自动跟随

实时更新PC端GPS位置，发送给无人机，完成PC的跟随

# -*- coding: utf-8 -*-
'''
---------------------------------------------------------------------------
基于GPS跟随模式

---------------------------------------------------------------------------

'''
from __future__ import print_function

from dronekit import connect, VehicleMode, LocationGlobalRelative
import gps
import socket
import time
import sys


# 连接到SITL
connection_string = '127.0.0.1:14555'
print('Connecting to vehicle on: %s' % connection_string)
vehicle = connect(connection_string, wait_ready=True, timeout=300)


#起飞
def arm_and_takeoff(aTargetAltitude):
    #起飞检查
    print("Basic pre-arm checks")
    #等待初始化
    while not vehicle.is_armable:
        print(" Waiting for vehicle to initialise...")
        time.sleep(1)

    #解锁电机
    print("Arming motors")
    #设定模式GUIDED
    vehicle.mode = VehicleMode("GUIDED")
    vehicle.armed = True    

    while not vehicle.armed:      
        print(" Waiting for arming...")
        time.sleep(1)

    #起飞到设定高度
    print("Taking off!")
    vehicle.simple_takeoff(aTargetAltitude) 

    #等待无人机到达设定高度
    while True:
        print(" Altitude: ", vehicle.location.global_relative_frame.alt)      
        if vehicle.location.global_relative_frame.alt>=aTargetAltitude*0.95: #Trigger just below target alt.
            print("Reached target altitude")
            break
        time.sleep(1)



try:

    #安装python gps接收笔记本的GPS位置
    gpsd = gps.gps(mode=gps.WATCH_ENABLE)

    #解锁起飞5m
    arm_and_takeoff(5)

    while True:
        
        if vehicle.mode.name != "GUIDED":
            print("User has changed flight modes - aborting follow-me")
            break    
            
        # 更新gps位置
        next(gpsd)

        #接收到gps位置后移动无人机
        if (gpsd.valid & gps.LATLON_SET) != 0:
            #跟随高度
            altitude = 30 
            #维度、经度、高度
            dest = LocationGlobalRelative(gpsd.fix.latitude, gpsd.fix.longitude, altitude)
            print("Going to: %s" % dest)
            #经纬度高度移动
            vehicle.simple_goto(dest)

            #每隔2s更新
            time.sleep(2)
            

#无gps时提示插入usb_gps或运行假的gps         
except socket.error:
    print("Error: gpsd service does not seem to be running, plug in USB GPS or run run-fake-gps.sh")
    sys.exit(1)

#关闭连接
print("Close vehicle object")
vehicle.close()


print("Completed")