Dynamic flight plans

As mentioned in Activity 11, another option for controlling unmanned aircraft is dynamic flight planning. In general, in the drone platform, we use a Raspberry py as an onboard computer. The programs for our onboard computer are written in Python 2.7 by importing the Dronekit library. In this phase of the project, we use a drone simulator of Mission Planner software to notice if our phyton program is working. On the other hand, we connect our program with the simulated drone by using the Dronekit library.

The diagram shown below illustrates the main algorithm of the program.

At the beginning of the program, it connects to the simulated drone in the Mission Planner using the Dronekit library. Then it directly checks if there is a valid mission (which can be any mission. For example, delivery service, ground surveying, etc… Important thing is that it has to include at least one waypoint and the mission has to be written to the autopilot by hitting “write” in the Mision Planner) available on the autopilot. If there is, it goes to the part of the program where it downloads the mission, executes take-off commands, and changes the mode to “Auto”. If there is no such mission on the autopilot, it waits.

Once the algorithm starts for the mission as auto mode, it goes through every element (waypoints, camera shots, servo motor, etc..) until the last waypoint. After reaching the last waypoint, it switched the autopilot to Return To Land mode and erases the current flight plan (So that it is ready for getting a new flight plan). Then it goes directly to the home location and lands. At the end of the program, it jumps to the beginning. Now, therefore, the program is ready for getting the new mission.

Outputs of the program is shown below.

In our case, we used the same flight plan as we used in activity 11 to investigate the interaction between the simulator drone and the python program. As result, we include the complete mission (Ground serveying) in the following photo.

Source code:

from dronekit import connect, VehicleMode, LocationGlobalRelative, Command, LocationGlobal
import time
from pymavlink import mavutil

def arm_and_takeoff(altitude):

   while not vehicle.is_armable:
      print("waiting to be armable")
      time.sleep(1)

   print("Arming motors")
   vehicle.mode = VehicleMode("GUIDED")
   vehicle.armed = True

   while not vehicle.armed: time.sleep(1)

   print("Taking Off")
   vehicle.simple_takeoff(altitude)

   while True:
      v_alt = vehicle.location.global_relative_frame.alt
      print(">> Altitude = %.1f m"%v_alt)
      if v_alt >= altitude - 1.0:
          print("Target altitude reached")
          break
      time.sleep(1)

def remove_mission(vehicle):
    cmds = vehicle.commands
    vehicle.commands.clear()
    vehicle.flush()

    cmds = vehicle.commands
    cmds.download()
    cmds.wait_ready()
    

def download_mission(vehicle):
    print "Downloading mission"
    cmds = vehicle.commands
    cmds.download()
    cmds.wait_ready()
    
    missionList = []
    n_WP        = 0
    for wp in vehicle.commands:
        missionList.append(wp)
        n_WP += 1 
        
    return n_WP, missionList
    

def add_home_as_last_wp(vehicle, wp_Last_Latitude, wp_Last_Longitude, wp_Last_Altitude):
   
    cmds = vehicle.commands
    cmds.download()
    cmds.wait_ready()

    missionlist=[]
    for cmd in cmds:
        missionlist.append(cmd)

    wpLastObject = Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, 
                           wp_Last_Latitude, wp_Last_Longitude, wp_Last_Altitude)
    missionlist.append(wpLastObject)

    cmds.clear()

    for cmd in missionlist:
        cmds.add(cmd)
    cmds.upload()
    
    return (cmds.count)    

def change_mode(vehicle, mode):
    while vehicle.mode != VehicleMode(mode):
            vehicle.mode = VehicleMode(mode)
            time.sleep(0.5)
    return True

def GND_mode(mode):
    if mode == 'GROUND':
        n_WP, missionList = download_mission(vehicle)
        time.sleep(2)
        if n_WP > 0:
            print ("A mission uploaded: takeoff!")
            mode = 'TAKEOFF'
    return mode

def TAKEOFF_mode(mode):
    if mode == 'TAKEOFF':
       
        add_home_as_last_wp(vehicle, vehicle.location.global_relative_frame.lat, vehicle.location.global_relative_frame.lon, vehicle.location.global_relative_frame.alt)
        print("Home waypoint added to the mission")
        time.sleep(1)
        arm_and_takeoff(10)
        print("Changing to AUTO")
        change_mode(vehicle,"AUTO")
        vehicle.groundspeed = gnd_speed
        mode = 'MISSION'
        print ("MISSION mode (AUTO)")
    return mode

def MISSION_mode(mode):
    if mode == 'MISSION':
        if vehicle.commands.next == vehicle.commands.count:
            print ("Final waypoint reached: go back home")
            remove_mission(vehicle)
            print ("Current mission removed")
            
            change_mode(vehicle,"RTL")
            mode = "BACK"
    return mode
def AT_HOME_mode(mode):
    if mode == "BACK":
        if vehicle.location.global_relative_frame.alt < 1:
            print ("GROUND mode, waiting for new missions")
            mode = 'GROUND'
    return mode
'''
#-------------- Connection
''' 
gnd_speed = 10 # [m/s]
mode      = 'GROUND'

print('Connecting...')
vehicle = connect('tcp:127.0.0.1:5762', wait_ready=True)


'''
#-------------- MAIN PROGRAMM  
'''   
while True:
    
    mode = GND_mode(mode)
    mode = TAKEOFF_mode(mode)      
    mode = MISSION_mode(mode)
    mode = AT_HOME_mode(mode)     

         
    
    time.sleep(0.5)