Help with simple code

[jruben4]

Trying to make some simple code to send thrust command to quad, and receive the actual thrust level back. For some reason, the full thrust ramp up/down gets completed, THEN all the log data comes in. How can I get continuous thrust log data even as new commands are sent?

import sys
sys.path.append("../lib")

import cflib.crtp

import logging
import time
from threading import Timer

import cflib.crtp
from cfclient.utils.logconfigreader import LogConfig
from cflib.crazyflie import Crazyflie

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

class LoggingExample:
"""
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 """

# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()

# Connect some callbacks from the Crazyflie API
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)

print "Connecting to %s" % link_uri

# Try to connect to the Crazyflie
self._cf.open_link(link_uri)

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

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."""
print "Connected to %s" % link_uri

# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=10)
#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")

# 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.
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 some variables are not in TOC")

thrust_mult = 1
thrust_step = 5000
thrust = 20000
pitch = 0
roll = 0
yawrate = 0

print "Ascent..."
while thrust <= 35000:
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
thrust += thrust_step
print "Thrust: ", thrust

print "Holding..."
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(1)
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(1)

print "Descent..."
while thrust > 20000:
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
thrust -= thrust_step
print "Thrust: ", thrust

self._cf.commander.send_setpoint(0, 0, 0, 0)

# Start a timer to disconnect in 10s
t = Timer(5, self._cf.close_link)
t.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 "Actual Thrust: %s" % (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)
self.is_connected = False

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
self.is_connected = False

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 = LoggingExample(available[0][0])
else:
print "No Crazyflies found, cannot run example"

# The Crazyflie 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 le.is_connected:
time.sleep(1)

[whoenig]

Please put the code within the code-environment next time.

The issue is that you are calling send_setpoint within the connected callback. Instead, execute the ramping code in another thread (and start the thread once you are connected).