Output pitch yaw roll while ramping up

[ChuchuZhang]

I am trying to output real time pitch, yaw and roll values to terminal while ramping up my crazyflie 2.0. I wrote a simple python program. I basically combined ramp.py and basiclog.py. I can get the output in the terminal but I always get some exception at the end of my program. In addition, sometimes my code just outputs the pitch, yaw and roll values but does not ramp up my crazyflie and sometimes it works for both.

Here is the exception:

Code: Select all

Exception in thread Thread-1 (most likely raised during interpreter shutdown):
Traceback (most recent call last):
  File "/usr/lib/python2.7/threading.py", line 810, in __bootstrap_inner
  File "../lib/cflib/crazyflie/__init__.py", line 357, in run
  File "../lib/cflib/crtp/radiodriver.py", line 158, in receive_packet
<type 'exceptions.AttributeError'>: 'NoneType' object has no attribute 'Empty'

Here is my code

Code: Select all

import time, sys
import logging
from threading import Thread
from threading import Timer
sys.path.append("../lib")
import cflib
from cflib.crazyflie import Crazyflie
import cflib.crtp
from cfclient.utils.logconfigreader import LogConfig
import logging
logging.basicConfig(level=logging.ERROR)

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._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):
        self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=30)
        self._lg_stab.add_variable("stabilizer.roll", "float")
        self._lg_stab.add_variable("stabilizer.pitch", "float")
        self._lg_stab.add_variable("stabilizer.yaw", "float")
        self._lg_stab.add_variable("stabilizer.thrust", "uint16_t")
        
        self._cf.log.add_config(self._lg_stab)
        if self._lg_stab.valid:
            # 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()
        else:
            print("Could not add logconfig since stabalizer variables are not in TOC")
	
	     Thread(target=self._ramp_motors).start()

    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\n" % (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 _ramp_motors(self):
        thrust_mult = 1
        thrust_step = 500
        thrust = 20000
        pitch = 0
        roll = 0
        yawrate = 0
	     self._cf.commander.send_setpoint(0, 0, 0, 0)
        while thrust >= 20000:
            self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
            time.sleep(0.1)
            if thrust >= 30000:
                thrust_mult = -1
            thrust += thrust_step * thrust_mult
        self._cf.commander.send_setpoint(0, 0, 0, 0)
        # Make sure that the last packet leaves before the link is closed
        # since the message queue is not flushed before closing
        time.sleep(0.1)
        self._cf.close_link()


if __name__ == '__main__':
    # Initialize the low-level drivers (don't list the debug drivers)
    cflib.crtp.init_drivers(enable_debug_driver=False)
    # 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:
	    le = MotorRampExample("radio://0/80/250K")
    else:
        print "No Crazyflies found, cannot run example"

Thanks!

[marcus]

Hi,

The exception you get when closing the example is connected to not properly closing down the link (see this issue). If the motors are not ramping up, are you sure that the calibration has succeeded? Check the RED led on the Crazyflie, if it's blinking at 1Hz the sensors hasn't calibrated and then the motors will not start. You will have to place the Crazyflie on a stable surface for the calibration to succeed, then the RED led will start blinking with 2 Hz (after quickly flashing).