basicparam.py

# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2014 Bitcraze AB
#
#  Espdrone Nano Quadcopter Client
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#  as published by the Free Software Foundation; either version 2
#  of the License, or (at your option) any later version.
#
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#  MA  02110-1301, USA.
"""
Simple example that connects to the first Espdrone found, triggers
reading of all the parameters and displays their values. It then modifies
one parameter and reads back it's value. Finally it disconnects.
"""
import logging
import random
import time
import argparse
import edlib.crtp
from edlib.espdrone import Espdrone

# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)


class ParamExample:
    """
    Simple logging example class that logs the Stabilizer from a supplied
    link uri and disconnects after 5s.
    """

    def __init__(self, link_uri):
        """ Initialize and run the example with the specified link_uri """

        self._ed = Espdrone(rw_cache='./cache')

        # Connect some callbacks from the Espdrone API
        self._ed.connected.add_callback(self._connected)
        self._ed.disconnected.add_callback(self._disconnected)
        self._ed.connection_failed.add_callback(self._connection_failed)
        self._ed.connection_lost.add_callback(self._connection_lost)

        print('Connecting to %s' % link_uri)

        # Try to connect to the Espdrone
        self._ed.open_link(link_uri)

        # Variable used to keep main loop occupied until disconnect
        self.is_connected = True

        self._param_check_list = []
        self._param_groups = []

        random.seed()

    def _connected(self, link_uri):
        """ This callback is called form the Espdrone API when a Espdrone
        has been connected and the TOCs have been downloaded."""
        print('Connected to %s' % link_uri)

        # Print the param TOC
        p_toc = self._ed.param.toc.toc
        for group in sorted(p_toc.keys()):
            print('{}'.format(group))
            for param in sorted(p_toc[group].keys()):
                print('\t{}'.format(param))
                self._param_check_list.append('{0}.{1}'.format(group, param))
            self._param_groups.append('{}'.format(group))
            # For every group, register the callback
            self._ed.param.add_update_callback(group=group, name=None,
                                               cb=self._param_callback)

        # You can also register a callback for a specific group.name combo
        self._ed.param.add_update_callback(group='cpu', name='flash',
                                           cb=self._cpu_flash_callback)

        print('')

    def _cpu_flash_callback(self, name, value):
        """Specific callback for the cpu.flash parameter"""
        print('The connected Espdrone has {}kb of flash'.format(value))

    def _param_callback(self, name, value):
        """Generic callback registered for all the groups"""
        print('{0}: {1}'.format(name, value))

        # Remove each parameter from the list and close the link when
        # all are fetched
        self._param_check_list.remove(name)
        if len(self._param_check_list) == 0:
            print('Have fetched all parameter values.')

            # First remove all the group callbacks
            for g in self._param_groups:
                self._ed.param.remove_update_callback(group=g,
                                                      cb=self._param_callback)

            # Create a new random value [0.00,1.00] for pid_attitude.pitch_kd
            # and set it
            pkd = random.random()
            print('')
            print('Write: pid_attitude.pitch_kd={:.2f}'.format(pkd))
            # When setting a value the parameter is automatically read back
            # and the registered callbacks will get the updated value
            self._ed.param.set_value('pid_attitude.pitch_kd',
                                     '{:.2f}'.format(pkd))
            self._ed.param.add_update_callback(group='pid_attitude',
                                               name='pitch_kd',
                                               cb=self._a_pitch_kd_callback)
            

    def _a_pitch_kd_callback(self, name, value):
        """Callback for pid_attitude.pitch_kd"""
        print('Readback: {0}={1}'.format(name, value))

        # End the example by closing the link (will cause the app to quit)
        self._ed.close_link()

    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Espdrone
        at the specified address)"""
        print('Connection to %s failed: %s' % (link_uri, msg))
        self.is_connected = False

    def _connection_lost(self, link_uri, msg):
        """Callback when disconnected after a connection has been made (i.e
        Espdrone moves out of range)"""
        print('Connection to %s lost: %s' % (link_uri, msg))

    def _disconnected(self, link_uri):
        """Callback when the Espdrone is disconnected (called in all cases)"""
        print('Disconnected from %s' % link_uri)
        self.is_connected = False


if __name__ == '__main__':
    # Initialize the low-level drivers (don't list the debug drivers)
    edlib.crtp.init_drivers(enable_debug_driver=False)
    parser = argparse.ArgumentParser()  
    parser.add_argument("--uri", help='The ip address of the drone, e.g. "192.168.0.102"')
    args = parser.parse_args()
    if args.uri:
        pe = ParamExample(args.uri)
    else:
        available = edlib.crtp.scan_interfaces()
        print('Espdrones found:')
        if available:
            print(available[0])
            pe = ParamExample(available[0][0])
        else:
            quit()

    # The Espdrone lib doesn't contain anything to keep the application
    # alive, so this is where your application should do something. In our
    # case we are just waiting until we are disconnected.
    while pe.is_connected:
        time.sleep(1)