CrazyFlie hover

hover.py

# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2014 Bitcraze AB
#
#  Crazyflie 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 Crazyflie found, ramps up/down
the motors and disconnects.
"""

import time, sys
from threading import Thread

import cflib
from cflib.crazyflie import Crazyflie

import logging
from cfclient.utils.logconfigreader import LogConfig

logging.basicConfig(level=logging.ERROR)

import pygame as pg

class MotorRampExample:
    """Example that connects to a Crazyflie and ramps the motors up/down and
    the disconnects"""
    def __init__(self, link_uri):
        """ Initialize and run the example with the specified link_uri """
        self._nextThrottle = 0
        self._stop = False
        self._cf = Crazyflie()

        self._cf.connected.add_callback(self._connected)
        self._cf.disconnected.add_callback(self._disconnected)
        self._cf.connection_failed.add_callback(self._connection_failed)
        self._cf.connection_lost.add_callback(self._connection_lost)

        self._cf.open_link(link_uri)

        print "Connecting to %s" % link_uri

    def _connected(self, link_uri):
        """ This callback is called form the Crazyflie API when a Crazyflie
        has been connected and the TOCs have been downloaded."""

        # Start a separate thread to do the motor test.
        # Do not hijack the calling thread!
        Thread(target=self._run_motors).start()

        # The definition of the logconfig can be made before connecting
        self._lg_stab = LogConfig(name="Accel", period_in_ms=10)
        self._lg_stab.add_variable("acc.x", "float")
        self._lg_stab.add_variable("acc.y", "float")
        self._lg_stab.add_variable("acc.z", "float")

        # Adding the configuration cannot be done until a Crazyflie is
        # connected, since we need to check that the variables we
        # would like to log are in the TOC.
        try:
            self._cf.log.add_config(self._lg_stab)
            # This callback will receive the data
            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
            # This callback will be called on errors
            self._lg_stab.error_cb.add_callback(self._stab_log_error)
            # Start the logging
            #self._lg_stab.start()
        except KeyError as e:
            print "Could not start log configuration," \
                  "{} not found in TOC".format(str(e))
        except AttributeError:
            print "Could not add Stabilizer log config, bad configuration."

    def _stab_log_error(self, logconf, msg):
        """Callback from the log API when an error occurs"""
        print "Error when logging %s: %s" % (logconf.name, msg)

    def _stab_log_data(self, timestamp, data, logconf):
        """Callback froma the log API when data arrives"""
        print "[%d][%s]: %s" % (timestamp, logconf.name, data)

    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Crazyflie
        at the speficied address)"""
        print "Connection to %s failed: %s" % (link_uri, msg)

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

    def _disconnected(self, link_uri):
        """Callback when the Crazyflie is disconnected (called in all cases)"""
        print "Disconnected from %s" % link_uri


    def throttleUp(self, amnt=1500):
        self._nextThrottle += amnt
        if self._nextThrottle > 40000:
            self._nextThrottle = 40000
        if self._nextThrottle < 20000:
            self._nextThrottle = 20000

    def throttleDown(self, amnt=1500):
        self._nextThrottle -= amnt
        if self._nextThrottle < 20000:
            self._nextThrottle = 0;

    def stop(self):
        self._stop = True

    def _run_motors(self):
        pitch = 0
        roll = 0
        yawrate = 0

        #Unlock startup thrust protection
        self._cf.commander.send_setpoint(0, 0, 0, 0)

        while not self._stop:
            self._cf.commander.send_setpoint(roll, pitch, yawrate, self._nextThrottle)
            time.sleep(0.1)
        print "Closing link (please wait)..."
        self._cf.commander.send_setpoint(0, 0, 0, 0)
        self._cf.close_link()


def printAccel(screen, centerX=-1, centerY=-1):
    font = pg.font.Font(None, 36)
    text = font.render("Hello There", 1, (255,255,255))
    textpos = text.get_rect()

    if centerX >= 0:
        textpos.centerx = centerX
    if centerY >= 0:
        textpos.centery = centerY

    screen.blit(text, textpos)

if __name__ == '__main__':
    # Initialize the low-level drivers (don't list the debug drivers)
    cflib.crtp.init_drivers(enable_debug_driver=False)
    pg.init()
    # Scan for Crazyflies and use the first one found
    print "Scanning interfaces for Crazyflies..."
    available = cflib.crtp.scan_interfaces()
    print "Crazyflies found:"
    for i in available:
        print i[0]

    if len(available) > 0:
        screen = pg.display.set_mode((640, 480))
        le = MotorRampExample(available[0][0])
        done = False
        try:
            while not done:
                pg.time.delay(100)
                for event in pg.event.get():
                    if event.type == pg.QUIT or (event.type == pg.KEYDOWN and event.key == pg.K_ESCAPE):
                        done = True
                    elif event.type == pg.KEYDOWN:
                        print chr(event.key)
                        if event.key == ord('w'):
                            le.throttleUp()
                        if event.key == ord('s'):
                            le.throttleDown()
                printAccel(screen)
                pg.display.flip()
        finally:
            le.stop()

    else:
        print "No Crazyflies found, cannot run example"