HardWare/MobileNode/Programming: uav_auto_mode.py

#!/usr/bin/python

"""
This program sets the UAV in AUTO mode. It reads the GPS information and
writes the most recent entry to a file. This entry is read by the CentMesh
sensing APP which saves it to a server.
"""
import re, sys, os, socket, select, time
import tempfile
from datetime import datetime

sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '../lib'))

import mavlink_apm, FTL_util

# The following will be used in figuring out when to print GPS info
INTERVAL = 5
new_time = current_time = datetime.now()

#This sample application writes the current GPS reading to this file,
# overwriting the previous entry. This file is used by sensor application
file_gps_data_for_sensing_app = "/tmp/gps_data"

# Object used for accessing utility functions
FTL_util_obj = FTL_util.FTL_util()

MAX_SIZE = 1024
"""Utility function: checks if the given filename exists and warns the user
if there exists one"""
def check_file_name_exists(file_name_provided):
    if os.path.exists(file_name_provided):
        print "The provided file name %s exists. Do you wish to overwrite it?(y/n)" % file_name_provided
        while 1:
            data = sys.stdin.readline()
            if not data.lower() == 'y\n'.lower() and not data.lower() == 'n\n'.lower():
                print 'Please enter \"y\" or \"n\"'
                continue
            elif data.lower() == 'n':
                print "Quitting..please try again"
                sys.exit(1)
            else:
                with open(file_name_provided, "w") as file_reference:
                    file_reference.close()
                break

""" Print GPS info, this function is invoked every time a hearbeat message
is received. The function saves only if the last 'save' has happened
atleast 5 seconds ago.
Independent of this save, the value is written to a temporary file
/tmp/gps_data. This file is used by the sensing application"""
def save_gps_info (decoded_message, file_gps_info):
    global current_time, new_time, last_gps_info_saved, FTL_util_obj, MAX_SIZE
    global file_gps_data_for_sensing_app
    last_gps_info_saved = str(decoded_message)
    with tempfile.NamedTemporaryFile(
        'w', dir=os.path.dirname(file_gps_data_for_sensing_app), delete=False) as tf:
        tf.write(last_gps_info_saved)
        tempname = tf.name
    os.rename(tempname, file_gps_data_for_sensing_app)

    # Check if we need to save it to the provided file name
    new_time = datetime.now()
    if (new_time - current_time).seconds >= int(INTERVAL):
        print "Time to save!!!!"
        with open(file_gps_info, "a") as file_reference:
            file_reference.write("\n" + str(decoded_message))
            file_reference.close()
        # Update the current timers
        current_time = new_time

""" Get address of MAVProxy """
def get_mavproxy_address (mav_obj,mavproxy_sock):
    heartbeat_received = 'False'
    mavproxy_sock.setblocking(1)
    print "Waiting for heartbeat message..."
    while not heartbeat_received.lower() == 'TRUE'.lower():
        # Wait for heartbeat message to get the remote address
        # used by MAVProxy
        try:
            data_from_mavproxy,address_of_mavproxy = mavproxy_sock.recvfrom (MAX_SIZE)
        except socket.error as v:
            print "Exception when trying to obtain address of MAVProxy"
            print os.strerror(v.errno)
        decoded_message = mav_obj.decode(data_from_mavproxy)
        msg_id = decoded_message.get_msgId()
        if msg_id == mavlink_apm.MAVLINK_MSG_ID_HEARTBEAT:
            # Undo the change made
            heartbeat_received = 'True'
            print 'Got the address of MAV,  proceeding..'
            print address_of_mavproxy
            mavproxy_sock.setblocking(0)
    return address_of_mavproxy

""" The main function """
def main():
    # Use the global values for MAX_SIZE and INTERVAL
    global MAX_SIZE, INTERVAL
    file_gps_info = "" 
    if len(sys.argv) != 3:
        print "Usage: ./uav_auto_mode.py <MAVProxy port> <File_to_save_GPS_info>"
        sys.exit(1)
    
    mavproxy_port = int(sys.argv[1])
    print "MAVProxy port is %d" % mavproxy_port
    file_gps_info = sys.argv[2]
    try:
        HOST = ''
        # Create a server socket for MAVProxy
        mavproxy_sock = socket.socket (socket.AF_INET,socket.SOCK_DGRAM)
        print 'created UDP socket for MAVProxy'
        mavproxy_sock.setblocking(0)
        mavproxy_sock.bind((HOST,mavproxy_port))
        print 'Binding socket for MAVProxy connection'
        # Create the mavproxy object
        mav_obj = mavlink_apm.MAVLink (mavproxy_sock)

    except Exception as ex:
        template = "An exception of type {0} occured. Arguments:\n{1!r}"
        message = template.format(type(ex).__name__, ex.args)
        print message
        sys.exit(1)

    # File name check
    check_file_name_exists(file_gps_info)

    #Get the address of mavproxy
    address_of_mavproxy = get_mavproxy_address (mav_obj, mavproxy_sock)
    return_status = FTL_util_obj.set_mav_mode(FTL_util_obj.auto_mode,mav_obj, 
                                                mavproxy_sock, address_of_mavproxy)
    if return_status < 0:
        print "Error while setting mode, please check the parameters passed..."
        sys.exit(1)
    # ARM the UAV
    component_id = mavlink_apm.MAV_COMP_ID_SYSTEM_CONTROL
    # Same command for arming or disarming, arm_flag controls whether the UAV
    # armed or disarmed. arm_flag=1->arm, arm_flag=0->disarm
    command = mavlink_apm.MAV_CMD_COMPONENT_ARM_DISARM
    arm_flag = 1
    # Number of confirmations needed for this command. 0 means immediately
    confirmation = 0
    # Other parameters are ignored by this command and are to be set to zero.
    PARAM_IGNORE = 0
    msg = mav_obj.command_long_encode (1,component_id,command,confirmation,
                                        arm_flag,PARAM_IGNORE,PARAM_IGNORE,
                                        PARAM_IGNORE,PARAM_IGNORE,PARAM_IGNORE,
                                        PARAM_IGNORE)
    try:
        mavproxy_sock.sendto(msg.get_msgbuf(),(address_of_mavproxy))
    except socket.error as v:
        print "Exception when trying to ARM the copter:"
        print os.strerror(v.errno)
    print "ARMED"

    # 'Listen' passively for messages from MAVProxy and filter
    # messages with GPS information.
    list_read_sockets = [mavproxy_sock]
    list_write_sockets = []
    list_error_sockets = []
    while 1:
        readable, writable, error = select.select(list_read_sockets,
                                                  list_write_sockets,
                                                  list_error_sockets,
                                                  int(INTERVAL))
        if readable:
            # print "Data received from MAVProxy!!!!"
            data_from_mavproxy,address_of_mavproxy = mavproxy_sock.recvfrom (MAX_SIZE)
            decoded_message = mav_obj.decode(data_from_mavproxy)
            msg_id = decoded_message.get_msgId()
            # Check if this information is GPS information.
            if msg_id == mavlink_apm.MAVLINK_MSG_ID_GPS_RAW_INT:
                save_gps_info(decoded_message, file_gps_info)
        else:
            print 'select() timeout, continue...'
            continue

if __name__ == '__main__':
    main()