HardWare/MobileNode/Programming: FTL_util.py

#!/usr/bin/python
"""
This module has the utility functions required by both leader and follower.
These include common functionality like obtaining a mode from heartbeat message.
"""
import re, sys, os, socket, select, time

import mavlink_apm

class FTL_util:
    dict_mode_names = {}
    # mode names for APM
    # We can save this in the following way: x1,x2,x3 = A,B,C. But for 
    # readability sake, we stick to this format
    stabilize_mode = 'STABILIZE'
    auto_mode = 'AUTO'
    guided_mode = 'GUIDED'
    rtl_mode = 'RTL'
    land_mode = 'LAND'
    of_loiter_mode = 'OF_LOITER'
    alt_hold_mode = 'ALT_HOLD'
    loiter_mode = 'LOITER'
    position_mode = 'POSITION'
    circle_mode = 'CIRCLE'
    approach_mode = 'APPROACH'
    acro_mode = 'ACRO'
    MAX_SIZE = 1024
    """Constructor for the class. Takes nothing as argument and initializes
        the dictionary mapping for <custom_mode>->mode name"""
    def __init__(self):
        self.dict_mode_names[0] = self.stabilize_mode
        self.dict_mode_names[1] = self.acro_mode
        self.dict_mode_names[2] = self.alt_hold_mode
        self.dict_mode_names[3] = self.auto_mode
        self.dict_mode_names[4] = self.guided_mode
        self.dict_mode_names[5] = self.loiter_mode
        self.dict_mode_names[6] = self.rtl_mode
        self.dict_mode_names[7] = self.circle_mode
        self.dict_mode_names[8] = self.position_mode
        self.dict_mode_names[9] = self.land_mode
        self.dict_mode_names[10] = self.of_loiter_mode
        self.dict_mode_names[11] = self.approach_mode

    """Destructor for the class, we do nothing here """
    def __del__(self):
        pass

    """Internal function: Given the base mode and custom mode, this
    function function returns the string for relevant mode. We are doing
    this as a different function as we may need to add some functionality 
    here later.
    UPDATE: We are currently using only the custom_mode. We may update
    this part if the custom_mode is found to be insufficient"""
    def __return_mav_mode__(self, base_mode, custom_mode):
        if (custom_mode) in self.dict_mode_names:
            return self.dict_mode_names[custom_mode]
        else:
            return ""
   
    """ Internal function to get the custom_mode corresponding to a string. 
        If mode is not found, returns -1 """
    def __return_custom_mode__(self, mode_str):
        # Get the value of custom_mode corresponding to the string passed
        for key,value in self.dict_mode_names.items():
            if value == mode_str:
                return key
        return -1
        
    """ Function that receives the mode as a string and sets the mode.
        Returns 0 on success, -1 on failure"""
    def set_mav_mode(self,mode_str,mav_obj,mavproxy_sock,address_of_mavproxy):
        custom_mode = self.__return_custom_mode__(mode_str)

        # Basic error checking
        if custom_mode == -1 or not mav_obj or not mavproxy_sock or not address_of_mavproxy:
            return -1
        msg = mav_obj.set_mode_encode(1, 1, custom_mode)
        list_read_sockets = [mavproxy_sock]
        list_write_sockets = list_error_sockets = []
        # Do not proceed until the MAV is set to the given mode
        while 1:
            try:
                mavproxy_sock.sendto(msg.get_msgbuf(),(address_of_mavproxy))
                data_from_mavproxy,address_of_mavproxy = mavproxy_sock.recvfrom (self.MAX_SIZE)
            except socket.error as v:
                print "Exception when trying to set AUTO mode:"
                print os.strerror(v.errno)
                time.sleep(1)
                continue

            decoded_message = mav_obj.decode(data_from_mavproxy)
            msg_id = decoded_message.get_msgId()
            # we are interested only if this is the heartbeat
            if msg_id == mavlink_apm.MAVLINK_MSG_ID_HEARTBEAT:
                # Get the mode from the message
                mode = self.get_mav_mode(str(decoded_message))
                if mode.lower() == mode_str.lower():
                    print "Mode set as expected"
                    return 0
                else:
                    continue

        
    """ Function that receives the heartbeat message as a string 
        and extracts base_mode and custom_mode."""
    def get_mav_mode (self, heartbeat_message):
        print "heartbeat message is %s" % heartbeat_message
        # Get base_mode
        match_for_base_mode =  re.search(r'base_mode : (.+?),',heartbeat_message)
        if not match_for_base_mode:
            print "Error - no base_mode found, return!"
            return ""
        else:
           base_mode = match_for_base_mode.group(1) 

        # Get custom_mode
        match_for_custom_mode =  re.search(r'custom_mode : (.+?),',heartbeat_message)
        if not match_for_custom_mode:
            print "Error - no custom_mode found, return!"
            return ""
        else:
           custom_mode = match_for_custom_mode.group(1)
        return self.__return_mav_mode__(int(base_mode),int(custom_mode))