class Main:
def __init__(self, uri):
self.crazyflie = Crazyflie()
self.uri = uri
cflib.crtp.init_drivers()
# You may need to update this value if your Crazyradio uses a different frequency.
self.crazyflie.open_link(uri)
# Set up the callback when connected
self.crazyflie.connected.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self.pulse_command).start()
def pulse_command(self):
thrust_mult = 1
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
while thrust >= 20000:
self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
if thrust >= 25000:
thrust_mult = -1
thrust += thrust_step * thrust_mult
self.crazyflie.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.crazyflie.close_link()
class SyncCrazyflie:
def __init__(self, link_uri, cf=None):
""" Create a synchronous Crazyflie instance with the specified
link_uri """
if cf:
self.cf = cf
else:
self.cf = Crazyflie()
self._link_uri = link_uri
self._connect_event = Event()
self._is_link_open = False
self._error_message = None
self.cf.connected.add_callback(self._connected)
self.cf.connection_failed.add_callback(self._connection_failed)
self.cf.disconnected.add_callback(self._disconnected)
def open_link(self):
if (self.is_link_open()):
raise Exception('Link already open')
print('Connecting to %s' % self._link_uri)
self.cf.open_link(self._link_uri)
self._connect_event.wait()
if not self._is_link_open:
raise Exception(self._error_message)
def __enter__(self):
self.open_link()
return self
def close_link(self):
self.cf.close_link()
self._is_link_open = False
def __exit__(self, exc_type, exc_val, exc_tb):
self.close_link()
def is_link_open(self):
return self._is_link_open
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)
self._is_link_open = True
self._connect_event.set()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self._is_link_open = False
self._error_message = msg
self._connect_event.set()
def _disconnected(self, link_uri):
self._is_link_open = False
class Main:
def __init__(self, uri):
self.crazyflie = Crazyflie()
self.uri = uri
cflib.crtp.init_drivers()
# You may need to update this value if your Crazyradio uses a different frequency.
self.crazyflie.open_link(uri)
# Set up the callback when connected
self.crazyflie.connected.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self.pulse_command).start()
def pulse_command(self):
thrust_mult = 1
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
while thrust >= 20000:
self.crazyflie.commander.send_setpoint(roll, pitch, yawrate,
thrust)
time.sleep(0.1)
if thrust >= 25000:
thrust_mult = -1
thrust += thrust_step * thrust_mult
self.crazyflie.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.crazyflie.close_link()
class QuadController(object):
"""Connects to a Crazyflie and controls from the leap."""
def __init__(self, link_uri, socket_reader):
self.socket_reader = socket_reader
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."""
try:
self._run()
except Exception as ex:
print(ex)
print("Run method raised an exception. Shutting down.")
finally:
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
self._cf.close_link()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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 _run(self):
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
while True:
rroll, rpitch, ryaw, rthrust = self.socket_reader.read(default=(0,0,0,0))
roll = int(rroll * 60)
pitch = int(rpitch * 60)
yaw = int(ryaw * 120)
thrust = int(map_linear(rthrust, 0, 1.0, 0, 40000))
thrust = bound(thrust, 0, 0xffff)
vector = (roll, pitch, yaw, thrust)
print("\t".join(map("{:.2f}".format, vector)))
self._cf.commander.send_setpoint(*vector)
def _tween(self, start, end, x):
"""A value goes from start to end and x is in [0,1]"""
x = float(x)
return start + (end - start) * x
class Main:
"""
Class is required so that methods can access the object fields.
"""
def __init__(self):
"""
Connect to Crazyflie, initialize drivers and set up callback.
The callback takes care of logging the accelerometer values.
"""
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
self.crazyflie.open_link("radio://0/8/250K")
def connectSetupFinished(self, linkURI):
# Set accelerometer logging config
motor_log_conf = LogConfig("motor", 10)
motor_log_conf.addVariable(LogVariable("motor.m4", "int32_t"))
motor_log_conf.addVariable(LogVariable("motor.m1", "int32_t"))
motor_log_conf.addVariable(LogVariable("motor.m2", "int32_t"))
motor_log_conf.addVariable(LogVariable("motor.m3", "int32_t"))
# Now that the connection is established, start logging
self.motor_log = self.crazyflie.log.create_log_packet(motor_log_conf)
if self.motor_log is not None:
self.motor_log.dataReceived.add_callback(self.log_motor_data)
self.motor_log.start()
else:
print("motor.m1/m2/m3/m4 not found in log TOC")
self.crazyflie.close_link()
def log_motor_data(self,data):
thrust_mult = 3
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
while thrust >= 20000:
self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
if (thrust >= 50000):
time.sleep(.01)
thrust_mult = -1
thrust = thrust + (thrust_step * thrust_mult)
self.crazyflie.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)
def log_motor_data(self, data):
logging.info("Motors: m1=%.2f, m2=%.2f, m3=%.2f, m4=%.2f" % (data["motor.m1"], data["motor.m2"], data["motor.m3"], data["motor.m4"]))
class Main:
def __init__(self):
self._stop = 0
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
# You may need to update this value if your Crazyradio uses a different frequency.
self.crazyflie.open_link("radio://0/10/250K")
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
lg = LogConfig("Magnetometer", 100)
lg.addVariable(LogVariable("mag.x", "int16_t"))
lg.addVariable(LogVariable("mag.y", "int16_t"))
lg.addVariable(LogVariable("mag.z", "int16_t"))
log = self.crazyflie.log.create_log_packet(lg)
if log is not None:
log.dataReceived.add_callback(self.magData)
log.start()
else:
print "Magnetometer not found in log TOC"
# Keep the commands alive so the firmware kill-switch doesn't kick in.
Thread(target=self.pulse_command).start()
Thread(target=self.input_loop).start()
def stop(self):
# Exit command received
# Set thrust to zero to make sure the motors turn off NOW
# make sure we actually set the thrust to zero before we kill the thread that sets it
time.sleep(0.5)
self._stop = 1;
# Exit the main loop
time.sleep(1)
self.crazyflie.close_link() # This errors out for some reason. Bad libusb?
sys.exit(0)
def input_loop(self):
command = raw_input("")
self.stop()
def pulse_command(self):
while 1:
self.crazyflie.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
# Exit if the parent told us to
if self._stop==1:
return
def magData(self, data):
x, y, z = data['mag.x'], data['mag.y'], data['mag.z']
try:
print x, y, z
sys.stdout.flush()
except:
self.stop()
def __init__(self):
# Ros stuff
rospy.init_node("crazyflie_server")
# Dictionaries to hold the crazyflies and related objects
# self._address = rospy.get_param('~address')
self._uris = rospy.get_param('~uris')
print(self._uris)
self._crazyflies = {}
self._crazyflie_logs = {}
self._controllers = {}
cflib.crtp.init_drivers(enable_debug_driver=False)
self._get_crazyflies_srv = rospy.Service('~get_crazyflies',
GetCrazyflies,
self._get_crazyflies_cb)
for name in self._uris.keys():
uri = self._uris[name]
parts = uri.split('/')
channel = parts[3]
cf = Crazyflie()
cf.connected.add_callback(self._connected)
cf.disconnected.add_callback(self._disconnected)
cf.connection_failed.add_callback(self._connection_failed)
cf.connection_lost.add_callback(self._connection_lost)
cf.open_link(uri)
self._crazyflies[uri] = (name, cf)
class Driver:
# Initial values, you can use these to set trim etc.
roll = 0.0
pitch = 0.0
yawrate = 0
thrust = 10001
def __init__(self, options=None):
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
rospy.loginfo("Waiting for crazyflie...")
while 1:
interfaces = cflib.crtp.scan_interfaces()
if len(interfaces) > 1:
radio = interfaces[0][0]
rospy.loginfo("Found: " + radio)
self.crazyflie.open_link(radio)
break
else:
rospy.sleep(1)
self.crazyflie.connectSetupFinished.add_callback(
self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
rospy.loginfo("Connected to " + linkURI)
class Driver:
# Initial values, you can use these to set trim etc.
roll = 0.0
pitch = 0.0
yawrate = 0
thrust = 10001
def __init__(self, options=None):
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
rospy.loginfo("Waiting for crazyflie...")
while 1:
interfaces = cflib.crtp.scan_interfaces()
if len(interfaces)>1:
radio = interfaces[0][0]
rospy.loginfo("Found: " + radio)
self.crazyflie.open_link(radio)
break
else:
rospy.sleep(1)
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
rospy.loginfo("Connected to " + linkURI)
def connect_to_crazyflie(self):
available_drones = cflib.crtp.scan_interfaces()
# Checking that we found the drone
if len(available_drones) == 0:
logging.error(
"Controller: Connect to crazyflie method could not find crazyflie"
)
raise Exception("Crazyflie cannot be found")
for drone in available_drones:
logging.debug("Controller: Found the following Drones: " +
drone[0])
# Creating a new crazyflie default object
crazyflie = Crazyflie(rw_cache='./cache')
# Setting up callbacks telling Crazyflie what to do
crazyflie.disconnected.add_callback(self.drone_disconnected)
crazyflie.connection_failed.add_callback(self.drone_connection_failed)
crazyflie.connection_lost.add_callback(self.drone_connection_lost)
logging.debug("Controller: Connecting to %s",
HomingDroneController.URI)
# Connecting to the crazyflie
crazyflie.open_link(HomingDroneController.URI)
# Variable to keep the python script running while connected
self.is_connected = True
return crazyflie
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):
""" 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._ramp_motors).start()
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#20000
thrustmax = 45000#25000
pitch = 0
roll = 0
yawrate = 0
while thrust >= 20000:
print "Thrust: "+str(round(thrust,2))
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
if thrust >= thrustmax:
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()
class HeadlessClient():
"""Crazyflie headless client"""
def __init__(self):
"""Initialize the headless client and libraries"""
cflib.crtp.init_drivers()
self._jr = JoystickReader(do_device_discovery=False)
self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache",
rw_cache=sys.path[1]+"/cache")
signal.signal(signal.SIGINT, signal.SIG_DFL)
def setup_controller(self, input_config, input_device=0):
"""Set up the device reader"""
# Set values for input from config (advanced)
self._jr.set_thrust_limits(
self._p2t(Config().get("min_thrust")),
self._p2t(Config().get("max_thrust")))
self._jr.set_rp_limit(Config().get("max_rp"))
self._jr.set_yaw_limit(Config().get("max_yaw"))
self._jr.set_thrust_slew_limiting(
self._p2t(Config().get("slew_rate")),
self._p2t(Config().get("slew_limit")))
# Set up the joystick reader
self._jr.device_error.add_callback(self._input_dev_error)
devs = self._jr.getAvailableDevices()
print "Will use [%s] for input" % devs[input_device]["name"]
self._jr.start_input(devs[input_device]["name"],
input_config)
def list_controllers(self):
"""List the available controllers"""
for dev in self._jr.getAvailableDevices():
print "Controller #{}: {}".format(dev["id"], dev["name"])
def connect_crazyflie(self, link_uri):
"""Connect to a Crazyflie on the given link uri"""
self._cf.connectionFailed.add_callback(self._connection_failed)
self._cf.open_link(link_uri)
self._jr.input_updated.add_callback(self._cf.commander.send_setpoint)
def _connection_failed(self, link, message):
"""Callback for a failed Crazyflie connection"""
print "Connection failed on {}: {}".format(link, message)
self._jr.stop_input()
sys.exit(-1)
def _input_dev_error(self, message):
"""Callback for an input device error"""
print "Error when reading device: {}".format(message)
sys.exit(-1)
def _p2t(self, percentage):
"""Convert a percentage to raw thrust"""
return int(65000 * (percentage / 100.0))
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(rw_cache='./cache')
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._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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 = 30000
lower_limit = 20000
pitch = -1
roll = 0.66
yawrate = 0
threshold_thrust = 26000
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(1)
self._cf.commander.send_setpoint(0, 0, 0, thrust)
time.sleep(0)
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
self._cf.close_link()
class AltHold:
def __init__(self, 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):
Thread(target=self._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
print("Connection to %s failed: %s" % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
print("Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
print("Disconnected from %s" % link_uri)
def _ramp_motors(self):
thrust = 36000 #Works at JoBa
pitch = 0
roll = 0
yawrate = 10
self._cf.commander.send_setpoint(0, 0, 0, 0)
print("Constant thrust")
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.75)
print("Turning on altitude hold")
self._cf.param.set_value("flightmode.althold", "True")
start_time = time.time()
timeC =time.time() - start_time
while timeC<3: #wait for 3 seconds
self._cf.commander.send_setpoint(0, 0, 0, 32767)
time.sleep(0.1)
print("hovering!")
timeC =time.time() - start_time
print("Closing link, time taken was: ")
print timeC
#close link and execute landing
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
self._cf.close_link()
class HeadlessClient():
"""Crazyflie headless client"""
def __init__(self):
"""Initialize the headless client and libraries"""
cflib.crtp.init_drivers()
self._jr = JoystickReader(do_device_discovery=False)
self._cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
signal.signal(signal.SIGINT, signal.SIG_DFL)
def setup_controller(self, input_config, input_device=0, xmode=False):
"""Set up the device reader"""
# Set up the joystick reader
self._jr.device_error.add_callback(self._input_dev_error)
print "Client side X-mode: %s" % xmode
if (xmode):
self._cf.commander.set_client_xmode(xmode)
devs = self._jr.getAvailableDevices()
print "Will use [%s] for input" % devs[input_device]["name"]
self._jr.start_input(devs[input_device]["name"], input_config)
def controller_connected(self):
""" Return True if a controller is connected"""
return True if (len(self._jr.getAvailableDevices()) > 0) else False
def list_controllers(self):
"""List the available controllers"""
for dev in self._jr.getAvailableDevices():
print "Controller #{}: {}".format(dev["id"], dev["name"])
def connect_crazyflie(self, link_uri):
"""Connect to a Crazyflie on the given link uri"""
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.param.add_update_callback(
group="imu_sensors",
name="HMC5883L",
cb=(lambda name, found: self._jr.setAltHoldAvailable(eval(found))))
self._jr.althold_updated.add_callback(
lambda enabled: self._cf.param.set_value("flightmode.althold",
enabled))
self._cf.open_link(link_uri)
self._jr.input_updated.add_callback(self._cf.commander.send_setpoint)
def _connection_failed(self, link, message):
"""Callback for a failed Crazyflie connection"""
print "Connection failed on {}: {}".format(link, message)
self._jr.stop_input()
sys.exit(-1)
def _input_dev_error(self, message):
"""Callback for an input device error"""
print "Error when reading device: {}".format(message)
sys.exit(-1)
class HeadlessClient():
"""Crazyflie headless client"""
def __init__(self):
"""Initialize the headless client and libraries"""
cflib.crtp.init_drivers()
self._jr = JoystickReader(do_device_discovery=False)
self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache",
rw_cache=sys.path[1]+"/cache")
signal.signal(signal.SIGINT, signal.SIG_DFL)
def setup_controller(self, input_config, input_device=0, xmode=False):
"""Set up the device reader"""
# Set up the joystick reader
self._jr.device_error.add_callback(self._input_dev_error)
print "Client side X-mode: %s" % xmode
if (xmode):
self._cf.commander.set_client_xmode(xmode)
devs = self._jr.getAvailableDevices()
print "Will use [%s] for input" % devs[input_device]["name"]
self._jr.start_input(devs[input_device]["name"],
input_config)
def controller_connected(self):
""" Return True if a controller is connected"""
return True if (len(self._jr.getAvailableDevices()) > 0) else False
def list_controllers(self):
"""List the available controllers"""
for dev in self._jr.getAvailableDevices():
print "Controller #{}: {}".format(dev["id"], dev["name"])
def connect_crazyflie(self, link_uri):
"""Connect to a Crazyflie on the given link uri"""
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.param.add_update_callback(group="imu_sensors", name="HMC5883L",
cb=(lambda name, found:
self._jr.setAltHoldAvailable(eval(found))))
self._jr.althold_updated.add_callback(
lambda enabled: self._cf.param.set_value("flightmode.althold", enabled))
self._cf.open_link(link_uri)
self._jr.input_updated.add_callback(self._cf.commander.send_setpoint)
def _connection_failed(self, link, message):
"""Callback for a failed Crazyflie connection"""
print "Connection failed on {}: {}".format(link, message)
self._jr.stop_input()
sys.exit(-1)
def _input_dev_error(self, message):
"""Callback for an input device error"""
print "Error when reading device: {}".format(message)
sys.exit(-1)
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):
""" 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._ramp_motors).start()
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
#m1list = [20000,25000,30000,35000,40000,45000,50000,55000,60000]
m1list = [20000]
m2 = 0
m3 = 0
m4 = 0
for m1 in m1list:
for i in range(2000):
time.sleep(0.01)
self._cf.commander.send_setpoint(0, 45, 0, m1)
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(5)
self._cf.close_link()
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):
Thread(target=self._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
print('Connection to %s failed: %s' % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
print('Disconnected from %s' % link_uri)
def _ramp_motors(self):
j = input('Choose a ramp: ')
thrust_mult = 1
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
print('Unlocked')
while thrust >= 20000:
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
print thrust
time.sleep(0.1)
if thrust >= j:
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()
class CrazyflieBridgedController():
def __init__(self, link_uri, serialport, jr, xmode=True):
"""Initialize the headless client and libraries"""
self._cf = Crazyflie()
self._jr = jr
self._serial = serialport
self._cf.commander.set_client_xmode(xmode)
self._cf.open_link(link_uri)
self.is_connected = True
self._console = ""
# Callbacks
self._cf.connected.add_callback(
lambda msg: print("Connected to " + msg))
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.console.receivedChar.add_callback(self._console_char)
self._cf.packet_received.add_callback(self._data_received)
# Controller callback
if self._jr:
self._jr.input_updated.add_callback(
self._cf.commander.send_setpoint)
self.serialthread = threading.Thread(target=self._serial_listener)
self.serialthread.start()
def _console_char(self, pk):
self._console = self._console + str(pk)
if "\n" in self._console:
print("Console: " + str(self._console), end="")
self._console = ""
def _serial_listener(self):
while self.is_connected:
data = self._serial.read(size=30)
if (data):
pk = CRTPPacket()
pk.port = CRTP_PORT_MICROROS
pk.data = data
self._cf.send_packet(pk)
def _data_received(self, pk):
if pk.port == CRTP_PORT_MICROROS:
self._serial.write(pk.data)
self._serial.flush()
def _disconnected(self, link_uri):
print("Disconnected. Reconnecting...")
# self._cf.open_link(link_uri)
self.is_connected = False
def _connection_failed(self, link_uri, msg):
print("Conection failed. Reconnecting...")
# self._cf.open_link(link_uri)
self.is_connected = False
class Input:
"""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):
""" 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._ramp_motors).start()
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
while thrust >= 20000:
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
if thrust >= 25000:
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()
class HeadlessClient():
"""Crazyflie headless client"""
def __init__(self):
"""Initialize the headless client and libraries"""
cflib.crtp.init_drivers()
self._jr = JoystickReader(do_device_discovery=False)
self._cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
signal.signal(signal.SIGINT, signal.SIG_DFL)
def setup_controller(self, input_config, input_device=0):
"""Set up the device reader"""
# Set values for input from config (advanced)
self._jr.set_thrust_limits(self._p2t(Config().get("min_thrust")),
self._p2t(Config().get("max_thrust")))
self._jr.set_rp_limit(Config().get("max_rp"))
self._jr.set_yaw_limit(Config().get("max_yaw"))
self._jr.set_thrust_slew_limiting(
self._p2t(Config().get("slew_rate")),
self._p2t(Config().get("slew_limit")))
# Set up the joystick reader
self._jr.device_error.add_callback(self._input_dev_error)
devs = self._jr.getAvailableDevices()
print "Will use [%s] for input" % devs[input_device]["name"]
self._jr.start_input(devs[input_device]["name"], input_config)
def list_controllers(self):
"""List the available controllers"""
for dev in self._jr.getAvailableDevices():
print "Controller #{}: {}".format(dev["id"], dev["name"])
def connect_crazyflie(self, link_uri):
"""Connect to a Crazyflie on the given link uri"""
self._cf.connectionFailed.add_callback(self._connection_failed)
self._cf.open_link(link_uri)
self._jr.input_updated.add_callback(self._cf.commander.send_setpoint)
def _connection_failed(self, link, message):
"""Callback for a failed Crazyflie connection"""
print "Connection failed on {}: {}".format(link, message)
self._jr.stop_input()
sys.exit(-1)
def _input_dev_error(self, message):
"""Callback for an input device error"""
print "Error when reading device: {}".format(message)
sys.exit(-1)
def _p2t(self, percentage):
"""Convert a percentage to raw thrust"""
return int(65000 * (percentage / 100.0))
class ConnectionTest:
"""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(rw_cache='./cache')
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)
self.is_connected = True
# change the parameters0,
self._param_check_list = []
self._param_groups = []
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)
# Start a timer to disconnect in 10s
t = Timer(10, self._cf.close_link)
t.start()
def _a_propTest_callback(self, name, value):
"""Callback for pid_attitude.pitch_kd"""
print('Readback: {0}={1}'.format(name, value))
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), flush=True)
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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)
class AppchannelTest:
"""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.appchannel.packet_received.add_callback(self._app_packet_received)
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._test_appchannel).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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 _app_packet_received(self, data):
(sum, ) = struct.unpack("<f", data)
print(f"Received sum: {sum}")
def _test_appchannel(self):
for i in range(10):
(x, y, z) = (i, i+1, i+2)
data = struct.pack("<fff", x, y, z)
self._cf.appchannel.send_packet(data)
print(f"Sent x: {x}, y: {y}, z: {z}")
time.sleep(1)
self._cf.close_link()
class Controller:
"""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):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print "Connected."
#self._quadcopter = Quadcopter (self._cf) # The older, harder-to-use code
self._quadcopter = QuadcopterAltHold (self._cf) # Altitude hold mode!
Thread(target=self._run).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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 _quality_updated(self, quality):
if quality < 30:
print "Low link quality: %s", quality
def _run(self):
print "Running..."
current_time = 0
self._cf.commander.send_setpoint (0,0,0,0);
while current_time <= 5:
print current_time
self._quadcopter._update(self._cf)
time.sleep(0.1)
current_time += 0.1
self._cf.close_link()
print "Done."
class KinectPilot():
"""Crazyflie Kinect Pilot"""
def __init__(self):
"""Initialize the headless client and libraries"""
cflib.crtp.init_drivers()
self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache",
rw_cache=sys.path[1]+"/cache")
self.kinect = Kinect(self)
# Create PID controllers for piloting the Crazyflie
# The Kinect camera is in the back of the Crazyflie so
# pitch = depth, roll = x, thrust = y
self.r_pid = PID_RP(P=0.05, D=1.0, I=0.00025, set_point=0.0)
self.p_pid = PID_RP(P=0.1, D=1.0, I=0.00025, set_point=0.0)
self.t_pid = PID(P=30.0, D=500.0, I=40.0, set_point=0.0)
self.sp_x = 320
self.sp_y = 240
signal.signal(signal.SIGINT, signal.SIG_DFL)
def connect_crazyflie(self, link_uri):
"""Connect to a Crazyflie on the given link uri"""
self._cf.connectionFailed.add_callback(self._connection_failed)
self._cf.open_link(link_uri)
def _connection_failed(self, link, message):
"""Callback for a failed Crazyflie connection"""
print "Connection failed on {}: {}".format(link, message)
sys.exit(-1)
def _p2t(self, percentage):
"""Convert a percentage to raw thrust"""
return int(65000 * (percentage / 100.0))
def set_sp_callback(self, x, y):
self.sp_x = x
self.sp_y = y
def control(self, dry=False):
"""Control loop for Kinect control"""
print "Attempting boot of Kinect"
safety = 10 # Prev was 10
temp = 0;
while True:
print "Kinect started"
(x,y,depth) = self.kinect.find_position()
print x,y,depth # Print out the initial x,y, and depth
print "Kinect finding position"
if x and y and depth:
safety = 10 # Previous was 10
print "Kinect PID updating."
class Main:
def __init__(self, uri):
self.crazyflie = Crazyflie()
self.uri = uri
cflib.crtp.init_drivers(enable_debug_driver=False)
# You may need to update this value if your Crazyradio uses a different frequency.
self.crazyflie.open_link(uri)
# Set up the callback when connected
self.crazyflie.connected.add_callback(self.connectSetupFinished)
self.crazyflie.disconnected.add_callback(self._disconnected)
self.crazyflie.connection_failed.add_callback(self._connection_failed)
self.crazyflie.connection_lost.add_callback(self._connection_lost)
def connectSetupFinished(self, linkURI):
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self.pulse_command).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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 pulse_command(self):
thrust_mult = 1
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
print("unlock crazyflie")
self.crazyflie.commander.send_setpoint(0, 0, 0, 0)
while thrust >= 20000:
self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
if thrust >= 25000:
thrust_mult = -1
thrust += thrust_step * thrust_mult
self.crazyflie.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)
print("command Ended")
self.crazyflie.close_link()
class Main:
"""
Class is required so that methods can access the object fields.
"""
def __init__(self):
"""
Connect to Crazyflie, initialize drivers and set up callback.
The callback takes care of logging the accelerometer values.
"""
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
self.crazyflie.connectSetupFinished.add_callback(
self.connectSetupFinished)
self.crazyflie.open_link("radio://0/8/250K")
def connectSetupFinished(self, linkURI):
"""
Configure the logger to log accelerometer values and start recording.
The logging variables are added one after another to the logging
configuration. Then the configuration is used to create a log packet
which is cached on the Crazyflie. If the log packet is None, the
program exits. Otherwise the logging packet receives a callback when
it receives data, which prints the data from the logging packet's
data dictionary as logging info.
"""
# Set accelerometer logging config
accel_log_conf = LogConfig("Accel", 10)
accel_log_conf.addVariable(LogVariable("acc.x", "float"))
accel_log_conf.addVariable(LogVariable("acc.y", "float"))
accel_log_conf.addVariable(LogVariable("acc.z", "float"))
accel_log_conf.addVariable(LogVariable("gyro.x", "float"))
accel_log_conf.addVariable(LogVariable("gyro.y", "float"))
accel_log_conf.addVariable(LogVariable("gyro.z", "float"))
# Now that the connection is established, start logging
self.accel_log = self.crazyflie.log.create_log_packet(accel_log_conf)
if self.accel_log is not None:
self.accel_log.dataReceived.add_callback(self.log_accel_data)
self.accel_log.start()
else:
print("acc.x/y/z not found in log TOC")
def log_accel_data(self, data):
logging.info("Accelerometer: x=%.2f, y=%.2f, z=%.2f; Gyrometer: x=%.2f, y=%.2f, z=%.2f" %
(data["acc.x"], data["acc.y"], data["acc.z"], data["gyro.x"], data["gyro.y"], data["gyro.z"]))
class Main:
"""
Class is required so that methods can baroess the object fields.
"""
def __init__(self):
"""
Connect to Crazyflie, initialize drivers and set up callback.
The callback takes care of logging the baroerometer values.
"""
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
self.crazyflie.connectSetupFinished.add_callback(
self.connectSetupFinished)
self.crazyflie.open_link("radio://0/10/250K")
def connectSetupFinished(self, linkURI):
"""
Configure the logger to log baroerometer values and start recording.
The logging variables are added one after another to the logging
configuration. Then the configuration is used to create a log packet
which is cached on the Crazyflie. If the log packet is None, the
program exits. Otherwise the logging packet receives a callback when
it receives data, which prints the data from the logging packet's
data dictionary as logging info.
"""
# Set baroerometer logging config
baro_log_conf = LogConfig("baro", 10)
baro_log_conf.addVariable(LogVariable("baro.asl", "float"))
baro_log_conf.addVariable(LogVariable("baro.aslRaw", "float"))
baro_log_conf.addVariable(LogVariable("baro.aslLong", "float"))
baro_log_conf.addVariable(LogVariable("baro.temp", "float"))
baro_log_conf.addVariable(LogVariable("baro.pressure", "float"))
# Now that the connection is established, start logging
self.baro_log = self.crazyflie.log.create_log_packet(baro_log_conf)
if self.baro_log is not None:
self.baro_log.dataReceived.add_callback(self.log_baro_data)
self.baro_log.start()
else:
print("baro.stuffs not found in log TOC")
def log_baro_data(self, data):
logging.info("Barometer: asl=%.2f, aslRaw=%.2f, aslLong=%.2f, temp=%.2f, pressure=%.2f" %
(data["baro.asl"], data["baro.aslRaw"], data["baro.aslLong"], data["baro.temp"], data["baro.pressure"]))
class Controller:
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.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.link_quality_updated.add_callback(self._quality_updated)
self._cf.open_link(link_uri)
self.quality = -1
self.failed = False
def linkQuality(self):
return self.quality
def connectionFailed(self):
return self.failed
def closeLink(self):
self._cf.close_link()
def __enter__(self):
return self
def __exit__(self, exc_type, exc_value, traceback):
self._cf.close_link()
def __del__(self):
self._cf.close_link()
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."""
self.failed = False
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
self.failed = True
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
self.failed = True
def _quality_updated(self, quality):
if quality < self.quality or self.quality == -1:
self.quality = quality
class Main:
"""
Class is required so that methods can baroess the object fields.
"""
def __init__(self):
"""
Connect to Crazyflie, initialize drivers and set up callback.
The callback takes care of logging the baroerometer values.
"""
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
self.crazyflie.open_link("radio://0/10/250K")
def connectSetupFinished(self, linkURI):
"""
Configure the logger to log baroerometer values and start recording.
The logging variables are added one after another to the logging
configuration. Then the configuration is used to create a log packet
which is cached on the Crazyflie. If the log packet is None, the
program exits. Otherwise the logging packet receives a callback when
it receives data, which prints the data from the logging packet's
data dictionary as logging info.
"""
# Set baroerometer logging config
baro_log_conf = LogConfig("baro", 10)
baro_log_conf.addVariable(LogVariable("baro.asl", "float"))
baro_log_conf.addVariable(LogVariable("baro.aslRaw", "float"))
baro_log_conf.addVariable(LogVariable("baro.aslLong", "float"))
baro_log_conf.addVariable(LogVariable("baro.temp", "float"))
baro_log_conf.addVariable(LogVariable("baro.pressure", "float"))
# Now that the connection is established, start logging
self.baro_log = self.crazyflie.log.create_log_packet(baro_log_conf)
if self.baro_log is not None:
self.baro_log.dataReceived.add_callback(self.log_baro_data)
self.baro_log.start()
else:
print("baro.stuffs not found in log TOC")
def log_baro_data(self, data):
logging.info(
"Barometer: asl=%.2f, aslRaw=%.2f, aslLong=%.2f, temp=%.2f, pressure=%.2f"
% (data["baro.asl"], data["baro.aslRaw"], data["baro.aslLong"], data["baro.temp"], data["baro.pressure"])
)
class LpsRebootToBootloader:
"""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):
""" 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._reboot_thread).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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 _reboot_thread(self):
anchors = LoPoAnchor(self._cf)
print('Sending reboot signal to all anchors 10 times in a row ...')
for retry in range(10):
for anchor_id in range(6):
anchors.reboot(anchor_id, anchors.REBOOT_TO_BOOTLOADER)
time.sleep(0.1)
self._cf.close_link()
class LpsRebootToBootloader:
"""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):
""" 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._reboot_thread).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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 _reboot_thread(self):
anchors = LoPoAnchor(self._cf)
print('Sending reboot signal to all anchors 10 times in a row ...')
for retry in range(10):
for anchor_id in range(6):
anchors.reboot(anchor_id, anchors.REBOOT_TO_BOOTLOADER)
time.sleep(0.1)
self._cf.close_link()
class MotorControl:
"""Class that connects to a Crazyflie and ramps the motors up/down and then disconnects"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
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._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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, cmd_vel):
roll = round(cmd_vel[0])
pitch = round(cmd_vel[1])
yawrate = round(cmd_vel[2])
thrust = round(cmd_vel[3])
print(cmd_vel)
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
class CrazyflieConnection:
def __init__(self, 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)
# Displays uri of Crazyflie to connect
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)
# Start a timer to disconnect in 10s
t = Timer(CF_ON_TIME, self._cf.close_link)
t.start()
# Starts getting data from the stream port
self.sp = StreamPort(cf_conn._cf, q, 29, CF_ON_TIME, SAMPLING_FREQ)
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
class MyCF:
def __init__(self, uri):
self.cf = Crazyflie()
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
self.cf.open_link(uri)
self.uri = uri
self.is_connected = False
def connected(self, uri):
self.is_connected = True
print("Connected to {}".format(uri))
def disconnected(self, uri):
print("disconnected from {}".format(uri))
def close(self):
self.cf.close_link()
def start_motors(self):
Thread(target=self.motor).start()
def motor(self):
thrust_mult = 1
thrust_step = 200
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
self.cf.commander.send_setpoint(0, 0, 0, 0)
while thrust >= 15000:
self.cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
sleep(0.1)
if thrust >= 25000:
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
sleep(0.1)
self.close()
class MyCF:
def __init__(self, uri):
self.cf = Crazyflie()
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
self.cf.open_link(uri)
self.uri = uri
self.is_connected = False
def connected(self, uri):
self.is_connected = True
print("Connected to {}".format(uri))
def disconnected(self, uri):
print("disconnected from {}".format(uri))
def close(self):
self.cf.close_link()
def start_motors(self):
Thread(target=self.motor).start()
def motor(self):
thrust_mult = 1
thrust_step = 200
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
self.cf.commander.send_setpoint(0, 0, 0, 0)
while thrust >= 15000:
self.cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
sleep(0.1)
if thrust >= 25000:
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
sleep(0.1)
self.close()
def main():
#show available links
cflib.crtp.init_drivers()
available = cflib.crtp.scan_interfaces()
for i in available:
print "Interface with URI [%s] found and name/comment [%s]" % (i[0],
i[1])
crazyflie = Crazyflie()
crazyflie.open_link("radio://0/10/250K")
roll = 0.0
pitch = 0.0
yawrate = 0
thrust = 35001
crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(5)
crazyflie.close_link()
class RaspSerial:
POSITION_CH = 0
def __init__(self, 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)
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):
print('Connected to %s' % link_uri)
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.POSITION_CH
pk.data = struct.pack('<fff', 3.1, 3.2, 3.3)
self._cf.send_packet(pk)
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
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))
class Main:
# Initial values, you can use these to set trim etc.
roll = 0.0
pitch = 0.0
yawrate = 0
thrust = 10001
_stop = 0;
def __init__(self):
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
# You may need to update this value if your Crazyradio uses a different frequency.
self.crazyflie.open_link("radio://0/1/250K")
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
print "Should be connected now...\n"
# Keep the commands alive so the firmware kill-switch doesn't kick in.
Thread(target=self.pulse_command).start()
print "Beginning input loop:"
while 1:
try:
command = raw_input("Set thrust (10001-60000) (0 will turn off the motors, e or q will quit):")
if (command=="exitstring manipulation python") or (command=="e") or (command=="quit") or (command=="q"):
# Exit command received
# Set thrust to zero to make sure the motors turn off NOW
self.thrust = 0
# make sure we actually set the thrust to zero before we kill the thread that sets it
time.sleep(0.5)
self._stop = 1;
# Exit the main loop
print "Exiting main loop in 1 second"
time.sleep(1)
self.crazyflie.close_link() # This errors out for some reason. Bad libusb?
break
class Main:
def __init__(self):
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
print("Trying to open link")
self.crazyflie.open_link("radio://0/10/250K")
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
time.sleep(10)
t1 = threading.Thread(target=self.FlyThread)
t1.start()
print("Link is opened")
def FlyThread(self):
roll = 0.0
pitch = 0.0
yawrate = 0
thrust = 10001 # minimum thrust required to power motors
print("Sending data")
# result = self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
for x in range(0, 500):
result = self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
# if (x % 50 == 0):
# print("Thrust: " + str(x))
print("Finished thrust increase")
def connectSetupFinished(self, linkURI):
"""
Called once connection is done
"""
print("Connection is finished")
class CrazyflieComm:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
start_thread = False
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
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 from the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
# Start a separate thread to do the motor control.
# Anything done in here will hijack the external thread
self.start_thread = True
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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)
class CrazyFlieController:
def __init__(self, link_uri):
self._crazyflie = Crazyflie()
self._crazyflie.connected.add_callback(self._connected)
self._crazyflie.open_link(link_uri)
self._crazyflie.commander.set_client_xmode(True)
def listen_command(self, roll, pitch, yaw, thrust):
print(roll, pitch, yaw, thrust * MAX_THRUST)
self._crazyflie.commander.send_setpoint(roll * 20, pitch * 20, yaw, thrust * MAX_THRUST)
time.sleep(0.001)
def _connected(self, link_uri):
thread = Thread(target=self.listen_command)
thread.start()
def _disconnected(self, link_uri):
print('Disconnected from %s ' % link_uri)
def _conntion_failed(self, link_uri, msg):
print('Connection to %s failed: %s' % (link_uri, msg))
class Main:
def __init__(self):
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
print("Trying to open link")
self.crazyflie.open_link("radio://0/10/250K")
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
time.sleep(10)
t1 = threading.Thread(target=self.FlyThread)
t1.start()
print("Link is opened")
def FlyThread(self):
roll = 0.0
pitch = 0.0
yawrate = 0
thrust = 10001 # minimum thrust required to power motors
print("In FlyThread")
#while(True):
for x in range(0,100):
result = self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust+x)
time.sleep(0.1)
self.crazyflie.close_link()
def connectSetupFinished(self, linkURI):
# Called when connection is done
print("Connection is finished");
class formationControl(threading.Thread):
""" A controller thread, taking references and mearsurements from the UAV
computing a control input"""
def __init__(self, link_uri):
threading.Thread.__init__(self)
#self.link_uri = link_uri
self.daemon = True
self.timePrint = 0.0
self.timeplt = time.time()
# Initialize the crazyflie object and add some callbacks
self._cf = Crazyflie(rw_cache='./cache')
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)
self.rate = 5 # (Hz) Rate of each sending control input
self.statefb_rate = 200 # (ms) Time between two consecutive state feedback
# Logged states -, attitude(p, r, y), rotation matrix
self.position = [0.0, 0.0, 0.0] # [m] in the global frame of reference
self.velocity = [0.0, 0.0,
0.0] # [m/s] in the global frame of reference
self.attitude = [0.0, 0.0,
0.0] # [rad] attitude with inverted roll (r)
self.rotMat = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
self.Mtr = [0.0, 0.0, 0.0, 0.0, 0.0] # Multiranger
# Limits
self.thrust_limit = (30000, 50000)
self.roll_limit = (-30.0, 30.0) # [Degree]
self.pitch_limit = (-30.0, 30.0) # [Degree]
self.yaw_limit = (-200.0, 200.0) # [Degree]
# Control setting
self.isEnabled = True
self.printDis = True
# System parameter
self.pi = 3.14159265359
# Control keyboard
self.keyboard_step = 0.1
self.keyboard_flag_a = 0.0
self.keyboard_flag_d = 0.0
self.keyboard_flag_w = 0.0
self.keyboard_flag_s = 0.0
self.keyboard_flag_j = 0.0
self.keyboard_flag_k = 0.0
def _run_controller(self):
""" Main control loop
# System parameters"""
m = 0.034 # [kg] actual mass with some decks
g = 9.799848 # [m/s^2] gravitational acceleration
timeSampling = 1.0 / float(self.rate) # [s] sampling time
number_Quad = 1 # [] number of crazyflie used
delta = 2 # [] if quad knows reference -> 1, otherwise 0
# Control Parameters
gv = 1.2 # for velocity
gp = 0.5 # for position
# Reference parameters
v0 = [0.0, 0.0, 0.0] # Reference velocity
r_amp = 0.5
r_omg = 0.2
r_k = 0.0
fp_ref = [0.0, 0.0, 0.0] # [m] the formation shape
fp = [0.0, 0.0, 0.0] # [m] the formation shape
fv = [0.0, 0.0, 0.0] # [m/s] the velocity formation shape
coef_init = 1.0 # initialize altitude
rot_mat = [
[0.0, 0.0, 0.0], # Rotation matrix for formation
[0.0, 0.0, 0.0], # shape in 3D space
[0.0, 0.0, 0.0]
]
col_thrs = 0.4
safe_thrs = 0.7
col_muy = 0.3
col_lda = -0.0
Mtr_pre = self.Mtr
Mtr_dot = [0.0, 0.0, 0.0, 0.0]
# z compensation
error_z = 0.0
error_z_pre = 0.0
z_ctrl_cp = 0.0
# Set the current reference to the current positional estimate
time.sleep(5)
# Unlock the controller
# First, we need send a signal to enable sending attitude reference and thrust force
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Hover at z = 0.3 durin 2 seconds
x_0, y_0, z_0 = self.position
yaw_0 = self.attitude[2]
timeStart = time.time()
while True:
self._cf.commander.send_position_setpoint(x=x_0,
y=y_0,
z=0.4,
yaw=yaw_0)
time.sleep(0.2)
if time.time() - timeStart > 2.0:
break
# Take a local coordinate
x_0, y_0, z_0 = self.position
yaw_d = self.attitude[2]
print('Initial position reference: ({}, {}, {})'.format(x_0, y_0, z_0))
x_d, y_d, z_d = [x_0, y_0, z_0]
# Begin while loop for sending the control signal
timeBeginCtrl = time.time()
while True:
timeStart = time.time()
# Change the reference velocity
if time.time() > timeBeginCtrl + 0.5:
#v0 = [0.0, -0.0, 0.0]
#v0[0] = r_omg * r_amp * cos(r_omg * r_k * timeSampling)
#v0[1] = - r_omg * r_amp * sin(r_omg * r_k * timeSampling)
rot_mat = [
[1.0, 0.0, 0.0], # Rotation matrix for formation
[0.0, 1.0, 0.0], # shape in 3D space
[0.0, 0.0, 1.0]
]
coef_init = 0.0
r_k = r_k + 1.0
if time.time() > timeBeginCtrl + 60.0:
self._cf.commander.send_setpoint(0, 0, 0, 39000)
self._cf.close_link()
print('Disconnect timeout')
# Get the reference
x_d = x_d + v0[0] * timeSampling
y_d = y_d + v0[1] * timeSampling
z_d = z_d + v0[2] * timeSampling
if self.keyboard_flag_s == 1.0:
x_d -= self.keyboard_step
self.keyboard_flag_s = 0.0
elif self.keyboard_flag_w == 1.0:
x_d += self.keyboard_step
self.keyboard_flag_w = 0.0
elif self.keyboard_flag_d == 1.0:
y_d -= self.keyboard_step
self.keyboard_flag_d = 0.0
elif self.keyboard_flag_a == 1.0:
y_d += self.keyboard_step
self.keyboard_flag_a = 0.0
elif self.keyboard_flag_j == 1.0:
z_d -= self.keyboard_step
self.keyboard_flag_j = 0.0
elif self.keyboard_flag_k == 1.0:
z_d += self.keyboard_step
self.keyboard_flag_k = 0.0
# Storage data
self.datalog_Pos.write(
str(time.time()) + "," + str(self.position[0]) + "," +
str(self.position[1]) + "," + str(self.position[2]) + "\n")
# Send set point
#self._cf.commander.send_setpoint(roll_d, pitch_d, yaw_d, thrust_d)
#self._cf.commander.send_setpoint(roll_d, pitch_d, yaw_d, 43000)
self._cf.commander.send_position_setpoint(x_d, y_d, z_d, yaw_d)
#self.print_at_period(2.0, message)
self.loop_sleep(timeStart)
# End
def _connected(self, link_uri):
""" This callback is called from the Crazyflie API when a Crazyflie
has been connected adn TOCs have been downloaded """
print('connected to: %s' % link_uri)
# Open text file for recording data
self.datalog_Pos = open("log_data/Cf2log_Pos", "w")
#self.datalog_Vel = open("log_data/Cf2log_Vel","w")
#self.datalog_Att = open("log_data/Cf2log_Att","w")
self.datalog_Att = open("log_data/Cf2log_Mtr", "w")
# Reset Kalman filter
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
print('Wait for Kalamn filter')
# Feedback position estimated by KF
logPos = LogConfig(name='Kalman Position',
period_in_ms=self.statefb_rate)
logPos.add_variable('kalman.stateX', 'float')
logPos.add_variable('kalman.stateY', 'float')
logPos.add_variable('kalman.stateZ', 'float')
# Feedback velocity estimated by KF
logVel = LogConfig(name='Kalman Velocity',
period_in_ms=self.statefb_rate)
logVel.add_variable('kalman.statePX', 'float')
logVel.add_variable('kalman.statePY', 'float')
logVel.add_variable('kalman.statePZ', 'float')
# Feedback Quaternion attitude estimated by KF
logAtt = LogConfig(name='Kalman Attitude',
period_in_ms=self.statefb_rate)
logAtt.add_variable('kalman.q0', 'float')
logAtt.add_variable('kalman.q1', 'float')
logAtt.add_variable('kalman.q2', 'float')
logAtt.add_variable('kalman.q3', 'float')
# Feedback Multi ranger
logMultiRa = LogConfig(name='Range', period_in_ms=self.statefb_rate)
logMultiRa.add_variable('range.front', 'uint16_t')
logMultiRa.add_variable('range.back', 'uint16_t')
logMultiRa.add_variable('range.left', 'uint16_t')
logMultiRa.add_variable('range.right', 'uint16_t')
logMultiRa.add_variable('range.up', 'uint16_t')
# Invoke logged states
self._cf.log.add_config(logPos)
self._cf.log.add_config(logVel)
self._cf.log.add_config(logAtt)
self._cf.log.add_config(logMultiRa)
# Start invoking logged states
if logPos.valid and logVel.valid:
# Position
logPos.data_received_cb.add_callback(self.log_pos_callback)
logPos.error_cb.add_callback(logging_error)
logPos.start()
# Velocity
logVel.data_received_cb.add_callback(self.log_vel_callback)
logVel.error_cb.add_callback(logging_error)
logVel.start()
# Quadternion attitude
logAtt.data_received_cb.add_callback(self.log_att_callback)
logAtt.error_cb.add_callback(logging_error)
logAtt.start()
# Multi-Ranger
logMultiRa.data_received_cb.add_callback(self.log_mtr_callback)
logMultiRa.error_cb.add_callback(logging_error)
logMultiRa.start()
else:
print(
"One or more of the variables in the configuration was not found"
+ "in log TOC. No logging will be possible")
# Start in a thread
threading.Thread(target=self._run_controller).start()
def log_pos_callback(self, timestamp, data, Logconf):
""" Callback for the logging the position of the UAV in the global frame"""
self.position = [
data['kalman.stateX'], data['kalman.stateY'], data['kalman.stateZ']
]
#self.datalog_Pos.write(str([time.time(), self.position]) + "\n")
#print('Position x, y, z, time', self.position, time.time())
#print(self.rate)
def log_att_callback(self, timestamp, data, Logconf):
""" Callback for the logging the quadternion attitude of the UAV which is
converted to roll, pitch, yaw in radians"""
q = [
data['kalman.q0'], data['kalman.q1'], data['kalman.q2'],
data['kalman.q3']
]
#Convert the quadternion to pitch roll and yaw
yaw = atan2(2 * (q[1] * q[2] + q[0] * q[3]),
q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3])
pitch = asin(-2 * (q[1] * q[3] - q[0] * q[2]))
roll = atan2(2 * (q[2] * q[3] + q[0] * q[1]),
q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3])
self.attitude = [roll, pitch, yaw]
# Convert the quaternion to a rotation matrix
R = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
R[0][0] = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]
R[0][1] = 2 * q[1] * q[2] - 2 * q[0] * q[3]
R[0][2] = 2 * q[1] * q[3] + 2 * q[0] * q[2]
R[1][0] = 2 * q[1] * q[2] + 2 * q[0] * q[3]
R[1][1] = q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3]
R[1][2] = 2 * q[2] * q[3] - 2 * q[0] * q[1]
R[2][0] = 2 * q[1] * q[3] - 2 * q[0] * q[2]
R[2][1] = 2 * q[2] * q[3] + 2 * q[0] * q[1]
R[2][2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]
self.rotMat = R
#self.datalog_Att.write(str([time.time(), self.attitude]) + "\n")
#print('Attitude r, p, y', self.attitude)
def log_vel_callback(self, timestamp, data, Logconf):
""" Callback for logging the velocity of the UAV defined w.r.t the body frame
this subsequently rotated to the global frame"""
PX, PY, PZ = [
data['kalman.statePX'], data['kalman.statePY'],
data['kalman.statePZ']
]
R = self.rotMat
self.velocity[0] = R[0][0] * PX + R[0][1] * PY + R[0][2] * PZ
self.velocity[1] = R[1][0] * PX + R[1][1] * PY + R[1][2] * PZ
self.velocity[2] = R[2][0] * PX + R[2][1] * PY + R[2][2] * PZ
#self.datalog_Vel.write(str([time.time(), self.velocity]) + "\n")
#print('Velocity rx, ry, rz', self.velocity)
def log_mtr_callback(self, timestamp, data, Logconf):
""" Callback for the logging the position of the UAV in the global frame"""
self.Mtr = [
data['range.front'] * 0.001,
data['range.back'] * 0.001,
data['range.left'] * 0.001,
data['range.right'] * 0.001,
data['range.up'] * 0.001,
]
#print('Time, Multirange: front, back, left, right, up', time.time(), self.Mtr)
def _connection_failed(self, link_uri, msg):
""" Callback when connection initial connection fails
there is no Crazyflie at the specified address """
print('Connection to %s failed: %s' % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
""" Callback when disconected after a connection has been made """
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
""" Callback when the Crazyflie is disconnected (called in all cases) """
if self.printDis:
print('Disconnected from %s' % link_uri)
self.printDis = False
def loop_sleep(self, timeStart):
""" Sleeps the control loop to make it run at a specified rate """
deltaTime = 1.0 / float(self.rate) - (time.time() - timeStart)
if deltaTime > 0:
time.sleep(deltaTime)
def _close_connection(self, message):
""" This is able to close link connecting Crazyflie from keyboard """
if message == "q":
self.isEnabled = False
# end
def set_reference(self, message):
""" Enables an incremental change in the reference and defines the
keyboard mapping (change to your preference, but if so, also make sure
to change the valid_keys attribute in the interface thread)"""
verbose = True
if message == "s":
self.keyboard_flag_s = 1.0
if message == "w":
self.keyboard_flag_w = 1.0
if message == "d":
self.keyboard_flag_d = 1.0
if message == "a":
self.keyboard_flag_a = 1.0
if message == "j":
self.keyboard_flag_j = 1.0
if message == "k":
self.keyboard_flag_k = 1.0
from cflib.crazyflie.log import LogConfig
URI = 'radio://0/80/250K'
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
# Called when the link is established and the TOCs (that are not
# cached) have been downloaded
connected = Caller()
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie()
cf.open_link(URI)
time.sleep(8.0)
cf.commander.send_setpoint(0, 0, 0, 0)
def poshold(cf, t, z):
steps = t * 10
for r in range(steps):
cf.commander.send_hover_setpoint(0, 0, 0, z)
time.sleep(0.1)
class Odometry(threading.Thread):
def __init__(self, cf):
class AltHoldExample:
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)
self.is_connected = True
# Variable used to keep main loop occupied until disconnect
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."""
print('Connected to %s' % link_uri)
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._hover_test).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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)
self.is_connected = False
def _hover_test(self):
print("sending initial thrust of 0")
self._cf.commander.send_setpoint(0,0,0,0);
time.sleep(0.5);
print("putting in althold")
self._cf.param.set_value("flightmode.althold","True")
print("Stay in althold for 7s")
it=0
self._cf.commander.send_setpoint(0,0,0,40000)
#self._cf.param.set_value("flightmode.althold","True")
while it<300:
#self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
#self._cf.commander.send_setpoint(0.66,-1,0,32767)
self._cf.commander.send_setpoint(0,0,0,32767)
self._cf.param.set_value("flightmode.althold","True")
#self._cf.param.set_value("flightmode.poshold","False")
time.sleep(0.01)
it+=1
print("Close connection")
self._cf.commander.send_setpoint(0,0,0,0)
self._cf.close_link()
class EEPROMExample:
"""
Simple example listing the EEPROMs found and writes the default values
in it.
"""
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)
mems = self._cf.mem.get_mems(MemoryElement.TYPE_I2C)
print("Found {} EEPOM(s)".format(len(mems)))
if len(mems) > 0:
print("Writing default configuration to"
" memory {}".format(mems[0].id))
elems = mems[0].elements
elems["version"] = 1
elems["pitch_trim"] = 0.0
elems["roll_trim"] = 0.0
elems["radio_channel"] = 80
elems["radio_speed"] = 0
elems["radio_address"] = 0xE7E7E7E7E7
mems[0].write_data(self._data_written)
def _data_written(self, mem, addr):
print("Data written, reading back...")
mem.update(self._data_updated)
def _data_updated(self, mem):
print("Updated id={}".format(mem.id))
print("\tType : {}".format(mem.type))
print("\tSize : {}".format(mem.size))
print("\tValid : {}".format(mem.valid))
print("\tElements : ")
for key in mem.elements:
print("\t\t{}={}".format(key, mem.elements[key]))
self._cf.close_link()
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))
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
class Communication:
def __init__(self):
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
available_radios = cflib.crtp.scan_interfaces()
radio = available_radios[0][0]
self.crazyflie.open_link(radio)
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
self.crazyflie.connectionFailed.add_callback(self.connectionLost)
self.crazyflie.connectionLost.add_callback(self.connectionLost)
self.acc_pub = rospy.Publisher("/crazyflie/Acceleration", Acceleration)
self.ori_pub = rospy.Publisher("/crazyflie/Orientation", Orientation)
self.ori_sub = rospy.Subscriber("/crazyflie/OrientationSetPoint", Orientation, self.set_point_callback)
def connectSetupFinished(self, linkURI):
rospy.loginfo("Connected on %s"%linkURI)
#set up logging
period_msec = 10
acc_conf = LogConfig("Accel", period_msec)
acc_conf.addVariable(LogVariable("acc.x", "float"))
acc_conf.addVariable(LogVariable("acc.y", "float"))
acc_conf.addVariable(LogVariable("acc.z", "float"))
ori_conf = LogConfig("Stabilizer", period_msec)
ori_conf.addVariable(LogVariable("stabilizer.pitch", "float"))
ori_conf.addVariable(LogVariable("stabilizer.yaw", "float"))
ori_conf.addVariable(LogVariable("stabilizer.roll", "float"))
ori_conf.addVariable(LogVariable("stabilizer.thrust", "float"))
#Set up acc logging
self.acc_log = self.crazyflie.log.create_log_packet(acc_conf)
self.orientation_log = self.crazyflie.log.create_log_packet(ori_conf)
if (self.acc_log != None):
self.acc_log.dataReceived.add_callback(self.acc_data_callback)
self.acc_log.start()
rospy.loginfo("Succesfully set up acc logging")
else:
rospy.logerr("Unable to set up acc logging")
if (self.orientation_log != None):
self.orientation_log.dataReceived.add_callback(self.orientation_data_callback)
self.orientation_log.start()
rospy.loginfo("Succesfully set up stabilizer logging")
else:
rospy.logerr("Unable to set up stabilizer logging")
def connectionLost(self, linkURI, errmsg):
rospy.loginfo("Connection lost on %s: %s"%(linkURI, errmsg))
exit()
def connectionFailed(self, linkURI):
rospy.logerr("Connection failed on %s"%linkURI)
exit()
def acc_data_callback(self, data):
#print data
#rospy.loginfo(str(data))
self.acc_pub.publish(data["acc.x"], data["acc.y"], data["acc.z"])
def orientation_data_callback(self, data):
#print data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.roll"], data["stabilizer.thrust"]
self.ori_pub.publish(data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.roll"], data["stabilizer.thrust"])
#rospy.loginfo(str(data))
def set_point_callback(self, data):
self.crazyflie.commander.send_setpoint(data.pitch, data.yaw, -1*data.roll, data.thrust)
class MainUI(QtGui.QMainWindow, main_window_class):
connectionLostSignal = pyqtSignal(str, str)
connectionInitiatedSignal = pyqtSignal(str)
batteryUpdatedSignal = pyqtSignal(int, object, object)
connectionDoneSignal = pyqtSignal(str)
connectionFailedSignal = pyqtSignal(str, str)
disconnectedSignal = pyqtSignal(str)
linkQualitySignal = pyqtSignal(int)
_input_device_error_signal = pyqtSignal(str)
_input_discovery_signal = pyqtSignal(object)
_log_error_signal = pyqtSignal(object, str)
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=GuiConfig()
.get("enable_debug_driver"))
# Create the connection dialogue
self.connectDialogue = ConnectDialogue()
# Create and start the Input Reader
self._statusbar_label = QLabel("Loading device and configuration.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader(cf=self.cf)
self._active_device = ""
self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._load_input_data()
self._update_input
ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
# Connections for the Connect Dialogue
self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link)
self.connectionDoneSignal.connect(self.connectionDone)
self.cf.connection_failed.add_callback(self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self.connectionFailed)
self._input_device_error_signal.connect(self.inputDeviceError)
self.joystickReader.device_error.add_callback(
self._input_device_error_signal.emit)
self._input_discovery_signal.connect(self.device_discovery)
self.joystickReader.device_discovery.add_callback(
self._input_discovery_signal.emit)
# Connect UI signals
self.menuItemConnect.triggered.connect(self.connectButtonClicked)
self.logConfigAction.triggered.connect(self.doLogConfigDialogue)
self.connectButton.clicked.connect(self.connectButtonClicked)
self.quickConnectButton.clicked.connect(self.quickConnect)
self.menuItemQuickConnect.triggered.connect(self.quickConnect)
self.menuItemConfInputDevice.triggered.connect(self.configInputDevice)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self.updateBatteryVoltage)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(self._open_config_folder)
self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(GuiConfig().get("auto_reconnect"))
# Do not queue data from the controller output to the Crazyflie wrapper
# to avoid latency
#self.joystickReader.sendControlSetpointSignal.connect(
# self.cf.commander.send_setpoint,
# Qt.DirectConnection)
self.joystickReader.input_updated.add_callback(
self.cf.commander.send_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(
self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self.connectionDone)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(
lambda linkURI: self.setUIState(UIState.DISCONNECTED,
linkURI))
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self.connectionLost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(
lambda linkURI: self.setUIState(UIState.CONNECTING,
linkURI))
self._log_error_signal.connect(self._logging_error)
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
# Set UI state in disconnected buy default
self.setUIState(UIState.DISCONNECTED)
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
# Add things to helper so tabs can access it
cfclient.ui.pluginhelper.cf = self.cf
cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
self._about_dialog = AboutDialog(cfclient.ui.pluginhelper)
self.menuItemAbout.triggered.connect(self._about_dialog.show)
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxes = []
self.toolboxesMenuItem.setMenu(QtGui.QMenu())
for t_class in cfclient.ui.toolboxes.toolboxes:
toolbox = t_class(cfclient.ui.pluginhelper)
dockToolbox = MyDockWidget(toolbox.getName())
dockToolbox.setWidget(toolbox)
self.toolboxes += [dockToolbox, ]
# Add menu item for the toolbox
item = QtGui.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.menu().addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# Load and connect tabs
self.tabsMenuItem.setMenu(QtGui.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtGui.QAction(tab.getMenuName(), self)
item.setCheckable(True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.menu().addAction(item)
tabItems[tab.getTabName()] = item
self.loadedTabs.append(tab)
if not tab.enabled:
item.setEnabled(False)
# First instantiate all tabs and then open them in the correct order
try:
for tName in GuiConfig().get("open_tabs").split(","):
t = tabItems[tName]
if (t != None and t.isEnabled()):
# Toggle though menu so it's also marked as open there
t.toggle()
except Exception as e:
logger.warning("Exception while opening tabs [%s]", e)
def setUIState(self, newState, linkURI=""):
self.uiState = newState
if (newState == UIState.DISCONNECTED):
self.setWindowTitle("Not connected")
self.menuItemConnect.setText("Connect to Crazyflie")
self.connectButton.setText("Connect")
self.menuItemQuickConnect.setEnabled(True)
self.batteryBar.setValue(3000)
self.linkQualityBar.setValue(0)
self.menuItemBootloader.setEnabled(True)
self.logConfigAction.setEnabled(False)
if (len(GuiConfig().get("link_uri")) > 0):
self.quickConnectButton.setEnabled(True)
if (newState == UIState.CONNECTED):
s = "Connected on %s" % linkURI
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.connectButton.setText("Disconnect")
self.logConfigAction.setEnabled(True)
if (newState == UIState.CONNECTING):
s = "Connecting to %s ..." % linkURI
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.connectButton.setText("Cancel")
self.quickConnectButton.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.menuItemQuickConnect.setEnabled(False)
@pyqtSlot(bool)
def toggleToolbox(self, display):
menuItem = self.sender().menuItem
dockToolbox = self.sender().dockToolbox
if display and not dockToolbox.isVisible():
dockToolbox.widget().enable()
self.addDockWidget(dockToolbox.widget().preferedDockArea(),
dockToolbox)
dockToolbox.show()
elif not display:
dockToolbox.widget().disable()
self.removeDockWidget(dockToolbox)
dockToolbox.hide()
menuItem.setChecked(False)
def _rescan_devices(self):
self._statusbar_label.setText("No inputdevice connected!")
self._menu_devices.clear()
self._active_device = ""
self.joystickReader.stop_input()
for c in self._menu_mappings.actions():
c.setEnabled(False)
devs = self.joystickReader.getAvailableDevices()
if (len(devs) > 0):
self.device_discovery(devs)
def configInputDevice(self):
self.inputConfig = InputConfigDialogue(self.joystickReader)
self.inputConfig.show()
def _auto_reconnect_changed(self, checked):
self._auto_reconnect_enabled = checked
GuiConfig().set("auto_reconnect", checked)
logger.info("Auto reconnect enabled: %s", checked)
def doLogConfigDialogue(self):
self.logConfigDialogue.show()
def updateBatteryVoltage(self, timestamp, data, logconf):
self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
cfclient.ui.pluginhelper.inputDeviceReader.inputdevice.setBatteryData(int(data["pm.vbat"] * 1000))
def connectionDone(self, linkURI):
self.setUIState(UIState.CONNECTED, linkURI)
GuiConfig().set("link_uri", linkURI)
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
self.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup loggingblock!")
def _logging_error(self, log_conf, msg):
QMessageBox.about(self, "Log error", "Error when starting log config"
" [%s]: %s" % (log_conf.name, msg))
def connectionLost(self, linkURI, msg):
if not self._auto_reconnect_enabled:
if (self.isActiveWindow()):
warningCaption = "Communication failure"
error = "Connection lost to %s: %s" % (linkURI, msg)
QMessageBox.critical(self, warningCaption, error)
self.setUIState(UIState.DISCONNECTED, linkURI)
else:
self.quickConnect()
def connectionFailed(self, linkURI, error):
if not self._auto_reconnect_enabled:
msg = "Failed to connect on %s: %s" % (linkURI, error)
warningCaption = "Communication failure"
QMessageBox.critical(self, warningCaption, msg)
self.setUIState(UIState.DISCONNECTED, linkURI)
else:
self.quickConnect()
def closeEvent(self, event):
self.hide()
self.cf.close_link()
GuiConfig().save_file()
def connectButtonClicked(self):
if (self.uiState == UIState.CONNECTED):
self.cf.close_link()
elif (self.uiState == UIState.CONNECTING):
self.cf.close_link()
self.setUIState(UIState.DISCONNECTED)
else:
self.connectDialogue.show()
def inputDeviceError(self, error):
self.cf.close_link()
QMessageBox.critical(self, "Input device error", error)
def _load_input_data(self):
self.joystickReader.stop_input()
# Populate combo box with available input device configurations
for c in ConfigManager().get_list_of_configs():
node = QAction(c,
self._menu_mappings,
checkable=True,
enabled=False)
node.toggled.connect(self._inputconfig_selected)
self.configGroup.addAction(node)
self._menu_mappings.addAction(node)
def _reload_configs(self, newConfigName):
# remove the old actions from the group and the menu
for action in self._menu_mappings.actions():
self.configGroup.removeAction(action)
self._menu_mappings.clear()
# reload the conf files, and populate the menu
self._load_input_data()
self._update_input(self._active_device, newConfigName)
def _update_input(self, device="", config=""):
self.joystickReader.stop_input()
self._active_config = str(config)
self._active_device = str(device)
GuiConfig().set("input_device", self._active_device)
GuiConfig().get(
"device_config_mapping"
)[self._active_device] = self._active_config
self.joystickReader.start_input(self._active_device,
self._active_config)
# update the checked state of the menu items
for c in self._menu_mappings.actions():
c.setEnabled(True)
if c.text() == self._active_config:
c.setChecked(True)
for c in self._menu_devices.actions():
c.setEnabled(True)
if c.text() == self._active_device:
c.setChecked(True)
# update label
if device == "" and config == "":
self._statusbar_label.setText("No input device selected")
elif config == "":
self._statusbar_label.setText("Using [%s] - "
"No input config selected" %
(self._active_device))
else:
self._statusbar_label.setText("Using [%s] with config [%s]" %
(self._active_device,
self._active_config))
def _inputdevice_selected(self, checked):
if (not checked):
return
self.joystickReader.stop_input()
sender = self.sender()
self._active_device = sender.text()
device_config_mapping = GuiConfig().get("device_config_mapping")
if (self._active_device in device_config_mapping.keys()):
self._current_input_config = device_config_mapping[
str(self._active_device)]
else:
self._current_input_config = self._menu_mappings.actions()[0].text()
GuiConfig().set("input_device", str(self._active_device))
for c in self._menu_mappings.actions():
if (c.text() == self._current_input_config):
c.setChecked(True)
self.joystickReader.start_input(str(sender.text()),
self._current_input_config)
self._statusbar_label.setText("Using [%s] with config [%s]" % (
self._active_device,
self._current_input_config))
def _inputconfig_selected(self, checked):
if (not checked):
return
self._update_input(self._active_device, self.sender().text())
def device_discovery(self, devs):
group = QActionGroup(self._menu_devices, exclusive=True)
for d in devs:
node = QAction(d["name"], self._menu_devices, checkable=True)
node.toggled.connect(self._inputdevice_selected)
group.addAction(node)
self._menu_devices.addAction(node)
if (d["name"] == GuiConfig().get("input_device")):
self._active_device = d["name"]
if (len(self._active_device) == 0):
self._active_device = self._menu_devices.actions()[0].text()
device_config_mapping = GuiConfig().get("device_config_mapping")
if (device_config_mapping):
if (self._active_device in device_config_mapping.keys()):
self._current_input_config = device_config_mapping[
str(self._active_device)]
else:
self._current_input_config = self._menu_mappings.actions()[0].text()
else:
self._current_input_config = self._menu_mappings.actions()[0].text()
# Now we know what device to use and what mapping, trigger the events
# to change the menus and start the input
for c in self._menu_mappings.actions():
c.setEnabled(True)
if (c.text() == self._current_input_config):
c.setChecked(True)
for c in self._menu_devices.actions():
if (c.text() == self._active_device):
c.setChecked(True)
def quickConnect(self):
try:
self.cf.open_link(GuiConfig().get("link_uri"))
except KeyError:
self.cf.open_link("")
def _open_config_folder(self):
QDesktopServices.openUrl(QUrl("file:///" + QDir.toNativeSeparators(sys.path[1])))
def closeAppRequest(self):
self.close()
sys.exit(0)
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')
# 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.')
# 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('[%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))
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
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):
""" 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._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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 take_off(self, roll, pitch, yawrate, thrust):
x = 0
while x <= 4:
#take off loop
x = x + 1
self._cf.commander.send_setpoint(roll, pitch, yawrate, 40000)
time.sleep(0.1)
return
def hover(self, roll, pitch, yawrate, thrust):
x = 0
while x <= 10:
#hover Loop
x = x + 1
self._cf.commander.send_setpoint(roll, pitch, yawrate, 32000)
time.sleep(0.1)
self._cf.commander.send_setpoint(roll, pitch, yawrate, 35000)
time.sleep(0.1)
return
def next_spot(self, roll, pitch, yawrate, thrust):
x = 0
while x <= 3:
#forward loop
x = x + 1
self._cf.commander.send_setpoint(roll, pitch, yawrate, 35000)
time.sleep(0.1)
#stop the quad from moving forward
pitch = -2
self._cf.commander.send_setpoint(roll, pitch, yawrate, 40000)
time.sleep(0.1)
pitch = 3
return
def land(self, roll, pitch, yawrate, thrust):
x = 0
thrust1 = 33000
while x <= 13:
#landing loop
x = x + 1
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust1)
time.sleep(0.1)
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust1)
time.sleep(0.1)
thrust1 = thrust1 - 100
return
def _ramp_motors(self):
thrust_mult = 1
thrust_step = 500
thrust = 35000
pitch = 3
roll = 1
yawrate = 0
y = 0
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
self.take_off(roll, pitch, yawrate, thrust)
while y == 0:
self.hover(roll, pitch, yawrate, thrust)
pitch = 10
self.next_spot(roll, pitch, yawrate, thrust)
pitch = 3
y = 1
self.hover(roll, pitch, yawrate, thrust)
thrust = 33000
self.land(roll, pitch, yawrate, thrust)
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()
class Main:
# Initial values, you can use these to set trim etc.
roll = 0.0
pitch = 0.0
yawrate = 0
thrust = 10001
_stop = 0;
def __init__(self):
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
# You may need to update this value if your Crazyradio uses a different frequency.
self.crazyflie.open_link("radio://0/10/250K")
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
print "Should be connected now...\n"
# Keep the commands alive so the firmware kill-switch doesn't kick in.
Thread(target=self.pulse_command).start()
print "Beginning input loop:"
while 1:
try:
command = raw_input("Set thrust (10001-60000) (0 will turn off the motors, e or q will quit):")
if (command=="exit") or (command=="e") or (command=="quit") or (command=="q"):
# Exit command received
# Set thrust to zero to make sure the motors turn off NOW
self.thrust = 0
# make sure we actually set the thrust to zero before we kill the thread that sets it
time.sleep(0.5)
self._stop = 1;
# Exit the main loop
print "Exiting main loop in 1 second"
time.sleep(1)
self.crazyflie.close_link() # This errors out for some reason. Bad libusb?
break
elif (self.is_number(command)):
# Good thrust value
self.thrust = int(command)
if self.thrust == 0:
self.thrust = 0
elif self.thrust <= 10000:
self.thrust = 10001
elif self.thrust > 60000:
self.thrust = 60000
print "Setting thrust to %i" % (self.thrust)
else:
print "Bad thrust value. Enter a number or e to exit"
except:
print "Exception thrown! Trying to continue!", sys.exc_info()[0]
def is_number(self, s):
try:
int(s)
return True
except ValueError:
return False
def pulse_command(self):
while 1:
self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yawrate, self.thrust)
time.sleep(0.1)
# Exit if the parent told us to
if self._stop==1:
print "Exiting keep alive thread"
return
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):
""" 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._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified 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 take_off(self, roll, pitch, yawrate, thrust):
x = 0
while x <= 4:
#take off loop
x = x+1
self._cf.commander.send_setpoint(roll, pitch, yawrate, 40000)
time.sleep(0.1)
return
def hover(self, roll, pitch, yawrate, thrust):
x = 0
while x <= 10:
#hover Loop
x = x+1
self._cf.commander.send_setpoint(roll, pitch, yawrate, 32000)
time.sleep(0.1)
self._cf.commander.send_setpoint(roll, pitch, yawrate, 35000)
time.sleep(0.1)
return
def next_spot(self, roll, pitch, yawrate, thrust):
x = 0
while x <= 3:
#forward loop
x = x+1
self._cf.commander.send_setpoint(roll, pitch , yawrate, 35000)
time.sleep(0.1)
#stop the quad from moving forward
pitch = -2
self._cf.commander.send_setpoint(roll, pitch, yawrate, 40000)
time.sleep(0.1)
pitch = 3
return
def land(self, roll, pitch, yawrate, thrust):
x = 0
thrust1 = 33000
while x <= 13:
#landing loop
x = x+1
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust1)
time.sleep(0.1)
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust1)
time.sleep(0.1)
thrust1 = thrust1 - 100
return
def _ramp_motors(self):
thrust_mult = 1
thrust_step = 500
thrust = 35000
pitch = 3
roll = 1
yawrate = 0
y = 0
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
self.take_off(roll, pitch, yawrate, thrust)
while y == 0:
self.hover(roll, pitch, yawrate, thrust)
pitch = 10
self.next_spot(roll, pitch, yawrate, thrust)
pitch = 3
y = 1
self.hover(roll, pitch, yawrate, thrust)
thrust = 33000
self.land(roll, pitch, yawrate, thrust)
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()
class UpDown:
"""docstring for UpDown"""
def __init__(self, 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._logger = LogConfig(name='Logger', period_in_ms=10)
self._logger.add_variable('acc.x', 'float')
self._logger.add_variable('acc.y', 'float')
self._logger.add_variable('acc.z', 'float')
self._logger.add_variable('acc.zw', 'float')
self._acc_x = 0
self._acc_y = 0
self._acc_z = 0
self._acc_zw = 0.0001
self.log = []
self.pidLog = []
self.acc_pid_x = None
self.acc_pid_y = None
self.acc_pid_z = None
self.vel_pid_x = None
self.vel_pid_y = None
self.vel_pid_z = None
print("Connecting to %s" % link_uri)
self._cf.open_link(link_uri)
self.is_connected = True
self.exit = False
self.init = False
Thread(target=self._exit_task).start()
Thread(target=self._run_task).start()
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."""
self.initTime = time.clock()
# self.log_file = open('log.txt', 'w')
# self.pid_log = open('pid.txt', 'w')
# self.log_file.write('[\n')
# self.pid_log.write('[\n')
try:
self._cf.log.add_config(self._logger)
# This callback will receive the data
self._logger.data_received_cb.add_callback(self._acc_log_data)
# This callback will be called on errors
self._logger.error_cb.add_callback(self._acc_log_error)
# Start the logging
self._logger.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Logger log config, bad configuration.')
def _acc_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _acc_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
self._acc_x = float(data['acc.x'])
self._acc_y = float(data['acc.y'])
self._acc_z = float(data['acc.z'])
self._acc_zw = data['acc.zw']
# self.log_file.write(json.dumps(data, sort_keys=True) + ",\n")
self.log.append(data)
self.init = True
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
# self.log_file.write(']')
# self.pid_log.write(']')
# self.log_file.close()
# self.pid_log.close()
with open('log.txt', 'w') as logFile:
json.dump(self.log, logFile)
with open('pid.txt', 'w') as pidFile:
json.dump(self.pidLog, pidFile)
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))
# self.log_file.write(']')
# self.pid_log.write(']')
# self.log_file.close()
# self.pid_log.close()
with open('log.txt', 'w') as logFile:
json.dump(self.log, logFile)
with open('pid.txt', 'w') as pidFile:
json.dump(self.pidLog, pidFile)
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
with open('log.txt', 'w') as logFile:
json.dump(self.log, logFile)
with open('pid.txt', 'w') as pidFile:
json.dump(self.pidLog, pidFile)
def _exit_task(self):
while not self.exit:
inp = int(input('Want to exit? [NO:0/YES:1]\n'))
if inp == 1:
self.exit = 1
def _run_task(self):
print("Running thread")
self._cf.commander.send_setpoint(0, 0, 0, 0)
while not self.exit:
if not self.init:
self.acc_pid_x = pid.PID(0, 0, 4.4, 0.05, 0.1, -1, 1)
self.acc_pid_y = pid.PID(0, 0, 4.4, 0.05, 0.1, -1, 1)
self.acc_pid_z = pid.PID(0, 0, 100, 0.0005, 0.1, 0.4, 2)
# Thread(target=self._log_pid).start()
continue
inp_acc_x = self._acc_x
self.out_acc_x, self.err_acc_x = self.acc_pid_x.compute(inp_acc_x)
self.setPitch = -self.out_acc_x*10
inp_acc_y = self._acc_y
self.out_acc_y, self.err_acc_y = self.acc_pid_y.compute(inp_acc_y)
self.setRoll = self.out_acc_y*10
inp_acc_z = self._acc_zw
self.out_acc_z, self.err_acc_z = self.acc_pid_z.compute(self._acc_zw)
self.setThrust = int(60000*self.out_acc_z/2)
self._thrust = self.setThrust
# data = {"IN_X": inp_acc_x, "OUT_X": out_acc_x,
# "IN_Y": inp_acc_y, "OUT_Y": out_acc_y,
# "IN_Z": self._acc_zw, "OUT_Z": out_acc_z,
# "OUT_Roll": setRoll, "OUT_Pitch": setPitch,
# "OUT_Thrust": setThrust}
# self.pidLog.append(data)
# self.pid_log.write(json.dumps(data, sort_keys=True) + ",\n")
self._cf.commander.send_setpoint(
self.setRoll, self.setPitch, 0, self._thrust)
time.sleep(0.01)
# self.log_file.write(']')
# self.pid_log.write(']')
# self.log_file.close()
# self.pid_log.close()
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(1)
self._cf.close_link()
def _log_pid(self):
data = {"IN_X": self._acc_x, "OUT_X": self.out_acc_x,
"IN_Y": self._acc_y, "OUT_Y": self.out_acc_y,
"IN_Z": self._acc_zw, "OUT_Z": self.out_acc_z,
"OUT_Roll": self.setRoll, "OUT_Pitch": self.setPitch,
"OUT_Thrust": self.setThrust}
self.pidLog.append(data)
class Main:
def __init__(self):
self.roll = 0.0
self.pitch = 0.0
self.yawrate = 0
self.thrust = 0
self._stop = 0;
self.series_index = -1
self.series_data = []
self.series_thrust = [0, 10001, 20001, 30001, 40001, 50001, 60000]
self.discard_data = True
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
# You may need to update this value if your Crazyradio uses a different frequency.
self.crazyflie.open_link("radio://0/10/250K")
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
print "Should be connected now...\n"
lg = LogConfig("Magnetometer", 100)
lg.addVariable(LogVariable("mag.x", "int16_t"))
lg.addVariable(LogVariable("mag.y", "int16_t"))
lg.addVariable(LogVariable("mag.z", "int16_t"))
log = self.crazyflie.log.create_log_packet(lg)
if log is not None:
log.dataReceived.add_callback(self.magData)
log.start()
else:
print "Magnetometer not found in log TOC"
# Keep the commands alive so the firmware kill-switch doesn't kick in.
Thread(target=self.pulse_command).start()
Thread(target=self.input_loop).start()
def stop(self):
# Exit command received
# Set thrust to zero to make sure the motors turn off NOW
self.thrust = 0
# make sure we actually set the thrust to zero before we kill the thread that sets it
time.sleep(0.5)
self._stop = 1;
self.process_data()
# Exit the main loop
print "Exiting main loop in 1 second"
time.sleep(1)
self.crazyflie.close_link() # This errors out for some reason. Bad libusb?
sys.exit(0)
def process_data(self):
centers = []
for series in self.series_data:
xs = []
ys = []
zs = []
for x, y, z in series:
# correct hard and soft iron errors
x = x - magn_ellipsoid_center[0]
y = y - magn_ellipsoid_center[1]
z = z - magn_ellipsoid_center[2]
prod = np.dot(magn_ellipsoid_transform, [[x], [y], [z]])
x = prod[0][0]
y = prod[1][0]
z = prod[2][0]
# store corrected values
xs.append(x)
ys.append(y)
zs.append(z)
center_x, center_y = fit_ellipse_center(xs, ys)
center_z = np.median(np.array(zs))
centers.append((center_x, center_y, center_z))
self.curve_fit(centers)
#self.show_plot()
def show_plot(self):
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
for series in self.series_data:
xs = []; ys = []; zs = []
for x, y, z in series:
xs.append(x)
ys.append(y)
zs.append(z)
ax.scatter(np.array(xs), np.array(ys), np.array(zs))
plt.show()
def curve_fit(self, centers):
nn = np.array([0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0])
xs = []
ys = []
zs = []
x0, y0, z0 = centers[0]
for i in range(0, 7):
x, y, z = centers[i]
xs.append(x0 - x)
ys.append(y0 - y)
zs.append(z0 - z)
print 'result'
print 'qx =', list(np.polyfit(nn, xs, 3))
print 'qy =', list(np.polyfit(nn, ys, 3))
print 'qz =', list(np.polyfit(nn, zs, 3))
def input_loop(self):
print "Beginning input loop:"
while True:
command = raw_input("Press Enter for next iteration (e or q will quit):")
if (command=="exit") or (command=="e") or (command=="quit") or (command=="q"):
self.stop()
elif command == '':
if self.series_index < 6:
self.discard_data = True
time.sleep(0.5)
# do stuff
self.series_index = self.series_index + 1
print 'Running next round, press rotate CF and hit enter when finished. Iteration', self.series_index
self.series_data.append([])
self.thrust = self.series_thrust[self.series_index]
time.sleep(0.5)
self.discard_data = False
else:
print 'Finished calibration'
self.stop()
def pulse_command(self):
while 1:
self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yawrate, self.thrust)
time.sleep(0.1)
# Exit if the parent told us to
if self._stop==1:
return
def magData(self, data):
x, y, z = data['mag.x'], data['mag.y'], data['mag.z']
if not self.discard_data:
self.series_data[self.series_index].append((x, y, z))
class MyFirstExample:
"""MyExamples"""
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)
# Variable used to keep main loop occupied until disconnect
#self.is_connected = True
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."""
#setup log config for sensor
#self._log_sensor = LogConfig("Logs", 200)
#self._log_sensor.add_variable("adc.vProx")
#self._log_sensor.add_variable("baro.aslLong")
#self._cf.log.add_config(self._log_sensor)
#if self._log_sensor.valid:
# This callback will receive the data
# self._log_sensor.data_received_cb.add_callback(self._log_data)
# This callback will be called on errors
# self._log_sensor.error_cb.add_callback(self._log_error)
# Start the logging
# self._log_sensor.start()
#else:
# print("Could not add logconfig since some variables are not in TOC")
# Start a timer to disconnect in 5s
print("Connected, starting timer")
t = Timer(7, self._landing)
t.start()
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._ramp_motors).start()
#self._ramp_motors()
print("sleeping 20")
time.sleep(20)
def _log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
#global ref_baro
#ref_baro = data["baro.aslLong"]
#print(ref_baro)
#print "[%d][%s]: %s" % (timestamp, logconf.name, data)
def _log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print "Error when logging %s: %s" % (logconf.name, msg)
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
def _landing(self):
thrust_mult = -1
thrust_step = 200
thrust = 20000
global landing
landing = True
print "Landing ON"
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._cf.param.set_value("flightmode.althold","False")
while thrust >= 15000:
self._cf.commander.send_setpoint(0, 0, 0, thrust)
thrust += thrust_step * thrust_mult
time.sleep(0.1)
print ("Landing thurst:",thrust)
time.sleep(0.1)
self._cf.close_link()
def _ramp_motors(self):
#thrust_mult = 1
#thrust_step = 100
thrust = 15000
#pitch = 0
#roll = 0
#yawrate = 0
global landing
landing = False
# print(ref_baro)
#while thrust >= 20000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# if thrust >= 25000:
# 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
# H O V E R
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
print ("Starting motors thurst:",thrust)
self._cf.commander.send_setpoint(0,0,0,thrust)
time.sleep(0.1)
print "Hover ON"
#self._cf.param.set_value("flightmode.althold","True")
while landing == False:
thrust = 32767
#self._cf.commander.send_setpoint(0,0,0,32767)
print ("Hovering thurst:",thrust)
time.sleep(0.1)
class MainUI(QtGui.QMainWindow, main_window_class):
connectionLostSignal = pyqtSignal(str, str)
connectionInitiatedSignal = pyqtSignal(str)
batteryUpdatedSignal = pyqtSignal(int, object, object)
connectionDoneSignal = pyqtSignal(str)
connectionFailedSignal = pyqtSignal(str, str)
disconnectedSignal = pyqtSignal(str)
linkQualitySignal = pyqtSignal(int)
_input_device_error_signal = pyqtSignal(str)
_input_discovery_signal = pyqtSignal(object)
_log_error_signal = pyqtSignal(object, str)
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
######################################################
### By lxrocks
### 'Skinny Progress Bar' tweak for Yosemite
### Tweak progress bar - artistic I am not - so pick your own colors !!!
### Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version,junk,machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10,10,0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info( "Found Yosemite:")
tcss = """
QProgressBar {
border: 2px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: #05B8CC;
}
"""
self.setStyleSheet(tcss)
else:
logger.info( "Pre-Yosemite")
######################################################
self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=Config()
.get("enable_debug_driver"))
# Create the connection dialogue
self.connectDialogue = ConnectDialogue()
# Create and start the Input Reader
self._statusbar_label = QLabel("Loading device and configuration.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._load_input_data()
ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
# Connections for the Connect Dialogue
self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link)
self.cf.connection_failed.add_callback(self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(self._display_input_device_error)
self.joystickReader.device_error.add_callback(
self._input_device_error_signal.emit)
self._input_discovery_signal.connect(self.device_discovery)
self.joystickReader.device_discovery.add_callback(
self._input_discovery_signal.emit)
# Connect UI signals
self.menuItemConnect.triggered.connect(self._connect)
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.connectButton.clicked.connect(self._connect)
self.quickConnectButton.clicked.connect(self._quick_connect)
self.menuItemQuickConnect.triggered.connect(self._quick_connect)
self.menuItemConfInputDevice.triggered.connect(self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_vbatt)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(self._open_config_folder)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
self.joystickReader.input_updated.add_callback(
self.cf.commander.send_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(
lambda linkURI: self._update_ui_state(UIState.DISCONNECTED,
linkURI))
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(
lambda linkURI: self._update_ui_state(UIState.CONNECTING,
linkURI))
self._log_error_signal.connect(self._logging_error)
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
# Set UI state in disconnected buy default
self._update_ui_state(UIState.DISCONNECTED)
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# Add things to helper so tabs can access it
cfclient.ui.pluginhelper.cf = self.cf
cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper)
self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
self._about_dialog = AboutDialog(cfclient.ui.pluginhelper)
self.menuItemAbout.triggered.connect(self._about_dialog.show)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show)
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxes = []
self.toolboxesMenuItem.setMenu(QtGui.QMenu())
for t_class in cfclient.ui.toolboxes.toolboxes:
toolbox = t_class(cfclient.ui.pluginhelper)
dockToolbox = MyDockWidget(toolbox.getName())
dockToolbox.setWidget(toolbox)
self.toolboxes += [dockToolbox, ]
# Add menu item for the toolbox
item = QtGui.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.menu().addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# Load and connect tabs
self.tabsMenuItem.setMenu(QtGui.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtGui.QAction(tab.getMenuName(), self)
item.setCheckable(True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.menu().addAction(item)
tabItems[tab.getTabName()] = item
self.loadedTabs.append(tab)
if not tab.enabled:
item.setEnabled(False)
# First instantiate all tabs and then open them in the correct order
try:
for tName in Config().get("open_tabs").split(","):
t = tabItems[tName]
if (t != None and t.isEnabled()):
# Toggle though menu so it's also marked as open there
t.toggle()
except Exception as e:
logger.warning("Exception while opening tabs [{}]".format(e))
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_mux, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m,
self._menu_mux,
checkable=True,
enabled=True)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_mux.addAction(node)
# TODO: Temporary
self._input_dev_stack = []
self._menu_mux.actions()[0].setChecked(True)
if Config().get("enable_input_muxing"):
self._menu_mux.setEnabled(True)
else:
logger.info("Input device muxing disabled in config")
self._mapping_support = True
def _update_ui_state(self, newState, linkURI=""):
self.uiState = newState
if newState == UIState.DISCONNECTED:
self.setWindowTitle("Not connected")
self.menuItemConnect.setText("Connect to Crazyflie")
self.connectButton.setText("Connect")
self.menuItemQuickConnect.setEnabled(True)
self.batteryBar.setValue(3000)
self._menu_cf2_config.setEnabled(False)
self._menu_cf1_config.setEnabled(True)
self.linkQualityBar.setValue(0)
self.menuItemBootloader.setEnabled(True)
self.logConfigAction.setEnabled(False)
if len(Config().get("link_uri")) > 0:
self.quickConnectButton.setEnabled(True)
if newState == UIState.CONNECTED:
s = "Connected on %s" % linkURI
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.connectButton.setText("Disconnect")
self.logConfigAction.setEnabled(True)
# Find out if there's an I2C EEPROM, otherwise don't show the
# dialog.
if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0:
self._menu_cf2_config.setEnabled(True)
self._menu_cf1_config.setEnabled(False)
if newState == UIState.CONNECTING:
s = "Connecting to {} ...".format(linkURI)
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.connectButton.setText("Cancel")
self.quickConnectButton.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.menuItemQuickConnect.setEnabled(False)
@pyqtSlot(bool)
def toggleToolbox(self, display):
menuItem = self.sender().menuItem
dockToolbox = self.sender().dockToolbox
if display and not dockToolbox.isVisible():
dockToolbox.widget().enable()
self.addDockWidget(dockToolbox.widget().preferedDockArea(),
dockToolbox)
dockToolbox.show()
elif not display:
dockToolbox.widget().disable()
self.removeDockWidget(dockToolbox)
dockToolbox.hide()
menuItem.setChecked(False)
def _rescan_devices(self):
self._statusbar_label.setText("No inputdevice connected!")
self._menu_devices.clear()
self._active_device = ""
self.joystickReader.stop_input()
for c in self._menu_mappings.actions():
c.setEnabled(False)
devs = self.joystickReader.available_devices()
if (len(devs) > 0):
self.device_discovery(devs)
def _show_input_device_config_dialog(self):
self.inputConfig = InputConfigDialogue(self.joystickReader)
self.inputConfig.show()
def _auto_reconnect_changed(self, checked):
self._auto_reconnect_enabled = checked
Config().set("auto_reconnect", checked)
logger.info("Auto reconnect enabled: {}".format(checked))
def _show_connect_dialog(self):
self.logConfigDialogue.show()
def _update_vbatt(self, timestamp, data, logconf):
self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
def _connected(self, linkURI):
self._update_ui_state(UIState.CONNECTED, linkURI)
Config().set("link_uri", str(linkURI))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
try:
self.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
def _logging_error(self, log_conf, msg):
QMessageBox.about(self, "Log error", "Error when starting log config"
" [{}]: {}".format(log_conf.name, msg))
def _connection_lost(self, linkURI, msg):
if not self._auto_reconnect_enabled:
if self.isActiveWindow():
warningCaption = "Communication failure"
error = "Connection lost to {}: {}".format(linkURI, msg)
QMessageBox.critical(self, warningCaption, error)
self._update_ui_state(UIState.DISCONNECTED, linkURI)
else:
self._quick_connect()
def _connection_failed(self, linkURI, error):
if not self._auto_reconnect_enabled:
msg = "Failed to connect on {}: {}".format(linkURI, error)
warningCaption = "Communication failure"
QMessageBox.critical(self, warningCaption, msg)
self._update_ui_state(UIState.DISCONNECTED, linkURI)
else:
self._quick_connect()
def closeEvent(self, event):
self.hide()
self.cf.close_link()
Config().save_file()
def _connect(self):
if self.uiState == UIState.CONNECTED:
self.cf.close_link()
elif self.uiState == UIState.CONNECTING:
self.cf.close_link()
self._update_ui_state(UIState.DISCONNECTED)
else:
self.connectDialogue.show()
def _display_input_device_error(self, error):
self.cf.close_link()
QMessageBox.critical(self, "Input device error", error)
def _load_input_data(self):
self.joystickReader.stop_input()
# Populate combo box with available input device configurations
for c in ConfigManager().get_list_of_configs():
node = QAction(c,
self._menu_mappings,
checkable=True,
enabled=False)
node.toggled.connect(self._inputconfig_selected)
self.configGroup.addAction(node)
self._menu_mappings.addAction(node)
def _reload_configs(self, newConfigName):
# remove the old actions from the group and the menu
for action in self._menu_mappings.actions():
self.configGroup.removeAction(action)
self._menu_mappings.clear()
# reload the conf files, and populate the menu
self._load_input_data()
self._update_input(self._active_device, newConfigName)
def _update_input(self, device="", config=""):
self._active_config = str(config)
self._active_device = str(device)
Config().set("input_device", str(self._active_device))
# update the checked state of the menu items
for c in self._menu_mappings.actions():
c.setEnabled(True)
if c.text() == self._active_config:
c.setChecked(True)
for c in self._menu_devices.actions():
c.setEnabled(True)
if c.text() == self._active_device:
c.setChecked(True)
# update label
if device == "" and config == "":
self._statusbar_label.setText("No input device selected")
elif config == "":
self._statusbar_label.setText("Using [{}] - "
"No input config selected".format
(self._active_device))
else:
self._statusbar_label.setText("Using [{}] with config [{}]".format
(self._active_device,
self._active_config))
def _mux_selected(self, checked):
if not checked:
return
selected_mux_name = str(self.sender().text())
self.joystickReader.set_mux(name=selected_mux_name)
logger.debug("Selected mux supports {} devices".format(self.joystickReader.get_mux_supported_dev_count()))
self._adjust_nbr_of_selected_devices()
def _get_saved_device_mapping(self, device_name):
"""Return the saved mapping for a given device"""
config = None
device_config_mapping = Config().get("device_config_mapping")
if device_name in device_config_mapping.keys():
config = device_config_mapping[device_name]
logging.debug("For [{}] we recommend [{}]".format(device_name, config))
return config
def _update_input_device_footer(self, device_name=None, mapping_name=None):
"""Update the footer in the bottom of the UI with status for the
input device and its mapping"""
if not device_name and not mapping_name:
self._statusbar_label.setText("No input device selected")
elif self._mapping_support and not mapping_name:
self._statusbar_label.setText("Using [{}] - "
"No input config selected".format(
device_name))
elif not self._mapping_support:
self._statusbar_label.setText("Using [{}]".format(device_name))
else:
self._statusbar_label.setText("Using [{}] with config [{}]".format(
device_name, mapping_name))
def _adjust_nbr_of_selected_devices(self):
nbr_of_selected = len(self._input_dev_stack)
nbr_of_supported = self.joystickReader.get_mux_supported_dev_count()
while len(self._input_dev_stack) > nbr_of_supported:
to_close = self._input_dev_stack.pop(0)
# Close and de-select it in the UI
self.joystickReader.stop_input(to_close)
for c in self._menu_devices.actions():
if c.text() == to_close:
c.setChecked(False)
def _inputdevice_selected(self, checked):
"""Called when a new input device has been selected from the menu"""
if not checked:
return
self._input_dev_stack.append(self.sender().text())
selected_device_name = str(self.sender().text())
self._active_device = selected_device_name
# Save the device as "last used device"
Config().set("input_device", str(selected_device_name))
# Read preferred config used for this controller from config,
# if found then select this config in the menu
self._mapping_support = self.joystickReader.start_input(selected_device_name)
self._adjust_nbr_of_selected_devices()
if self.joystickReader.get_mux_supported_dev_count() == 1:
preferred_config = self.joystickReader.get_saved_device_mapping(selected_device_name)
if preferred_config:
for c in self._menu_mappings.actions():
if c.text() == preferred_config:
c.setChecked(True)
def _inputconfig_selected(self, checked):
"""Called when a new configuration has been selected from the menu"""
if not checked:
return
selected_mapping = str(self.sender().text())
self.joystickReader.set_input_map(self._active_device, selected_mapping)
self._update_input_device_footer(self._active_device, selected_mapping)
def device_discovery(self, devs):
group = QActionGroup(self._menu_devices, exclusive=False)
for d in devs:
node = QAction(d.name, self._menu_devices, checkable=True)
node.toggled.connect(self._inputdevice_selected)
group.addAction(node)
self._menu_devices.addAction(node)
if d.name == Config().get("input_device"):
self._active_device = d.name
if len(self._active_device) == 0:
self._active_device = str(self._menu_devices.actions()[0].text())
device_config_mapping = Config().get("device_config_mapping")
if device_config_mapping:
if self._active_device in device_config_mapping.keys():
self._current_input_config = device_config_mapping[
self._active_device]
else:
self._current_input_config = self._menu_mappings.actions()[0]\
.text()
else:
self._current_input_config = self._menu_mappings.actions()[0].text()
# Now we know what device to use and what mapping, trigger the events
# to change the menus and start the input
for c in self._menu_devices.actions():
if c.text() == self._active_device:
c.setChecked(True)
for c in self._menu_mappings.actions():
c.setEnabled(True)
if c.text() == self._current_input_config:
c.setChecked(True)
def _quick_connect(self):
try:
self.cf.open_link(Config().get("link_uri"))
except KeyError:
self.cf.open_link("")
def _open_config_folder(self):
QDesktopServices.openUrl(QUrl("file:///" +
QDir.toNativeSeparators(sys.path[1])))
def closeAppRequest(self):
self.close()
sys.exit(0)
class MainUI(QtGui.QMainWindow, main_window_class):
connectionLostSignal = pyqtSignal(str, str)
connectionInitiatedSignal = pyqtSignal(str)
batteryUpdatedSignal = pyqtSignal(int, object, object)
connectionDoneSignal = pyqtSignal(str)
connectionFailedSignal = pyqtSignal(str, str)
disconnectedSignal = pyqtSignal(str)
linkQualitySignal = pyqtSignal(int)
_input_device_error_signal = pyqtSignal(str)
_input_discovery_signal = pyqtSignal(object)
_log_error_signal = pyqtSignal(object, str)
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache",
rw_cache=sys.path[1] + "/cache")
cflib.crtp.init_drivers(
enable_debug_driver=GuiConfig().get("enable_debug_driver"))
# Create the connection dialogue
self.connectDialogue = ConnectDialogue()
# Create and start the Input Reader
self._statusbar_label = QLabel("Loading device and configuration.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader(cf=self.cf)
self._active_device = ""
self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._load_input_data()
self._update_input
ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
# Connections for the Connect Dialogue
self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link)
self.connectionDoneSignal.connect(self.connectionDone)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self.connectionFailed)
self._input_device_error_signal.connect(self.inputDeviceError)
self.joystickReader.device_error.add_callback(
self._input_device_error_signal.emit)
self._input_discovery_signal.connect(self.device_discovery)
self.joystickReader.device_discovery.add_callback(
self._input_discovery_signal.emit)
# Connect UI signals
self.menuItemConnect.triggered.connect(self.connectButtonClicked)
self.logConfigAction.triggered.connect(self.doLogConfigDialogue)
self.connectButton.clicked.connect(self.connectButtonClicked)
self.quickConnectButton.clicked.connect(self.quickConnect)
self.menuItemQuickConnect.triggered.connect(self.quickConnect)
self.menuItemConfInputDevice.triggered.connect(self.configInputDevice)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self.updateBatteryVoltage)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(
GuiConfig().get("auto_reconnect"))
# Do not queue data from the controller output to the Crazyflie wrapper
# to avoid latency
#self.joystickReader.sendControlSetpointSignal.connect(
# self.cf.commander.send_setpoint,
# Qt.DirectConnection)
self.joystickReader.input_updated.add_callback(
self.cf.commander.send_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self.connectionDone)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(
lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI))
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self.connectionLost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(
lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI))
self._log_error_signal.connect(self._logging_error)
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
# Set UI state in disconnected buy default
self.setUIState(UIState.DISCONNECTED)
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
# Add things to helper so tabs can access it
cfclient.ui.pluginhelper.cf = self.cf
cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader
self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper)
self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
self._about_dialog = AboutDialog(cfclient.ui.pluginhelper)
self.menuItemAbout.triggered.connect(self._about_dialog.show)
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxes = []
self.toolboxesMenuItem.setMenu(QtGui.QMenu())
for t_class in cfclient.ui.toolboxes.toolboxes:
toolbox = t_class(cfclient.ui.pluginhelper)
dockToolbox = MyDockWidget(toolbox.getName())
dockToolbox.setWidget(toolbox)
self.toolboxes += [
dockToolbox,
]
# Add menu item for the toolbox
item = QtGui.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.menu().addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# Load and connect tabs
self.tabsMenuItem.setMenu(QtGui.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtGui.QAction(tab.getMenuName(), self)
item.setCheckable(True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.menu().addAction(item)
tabItems[tab.getTabName()] = item
self.loadedTabs.append(tab)
if not tab.enabled:
item.setEnabled(False)
# First instantiate all tabs and then open them in the correct order
try:
for tName in GuiConfig().get("open_tabs").split(","):
t = tabItems[tName]
if (t != None and t.isEnabled()):
# Toggle though menu so it's also marked as open there
t.toggle()
except Exception as e:
logger.warning("Exception while opening tabs [%s]", e)
def setUIState(self, newState, linkURI=""):
self.uiState = newState
if (newState == UIState.DISCONNECTED):
self.setWindowTitle("Not connected")
self.menuItemConnect.setText("Connect to Crazyflie")
self.connectButton.setText("Connect")
self.menuItemQuickConnect.setEnabled(True)
self.batteryBar.setValue(3000)
self.linkQualityBar.setValue(0)
self.menuItemBootloader.setEnabled(True)
self.logConfigAction.setEnabled(False)
if (len(GuiConfig().get("link_uri")) > 0):
self.quickConnectButton.setEnabled(True)
if (newState == UIState.CONNECTED):
s = "Connected on %s" % linkURI
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.connectButton.setText("Disconnect")
self.logConfigAction.setEnabled(True)
if (newState == UIState.CONNECTING):
s = "Connecting to %s ..." % linkURI
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.connectButton.setText("Cancel")
self.quickConnectButton.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.menuItemQuickConnect.setEnabled(False)
@pyqtSlot(bool)
def toggleToolbox(self, display):
menuItem = self.sender().menuItem
dockToolbox = self.sender().dockToolbox
if display and not dockToolbox.isVisible():
dockToolbox.widget().enable()
self.addDockWidget(dockToolbox.widget().preferedDockArea(),
dockToolbox)
dockToolbox.show()
elif not display:
dockToolbox.widget().disable()
self.removeDockWidget(dockToolbox)
dockToolbox.hide()
menuItem.setChecked(False)
def _rescan_devices(self):
self._statusbar_label.setText("No inputdevice connected!")
self._menu_devices.clear()
self._active_device = ""
self.joystickReader.stop_input()
for c in self._menu_mappings.actions():
c.setEnabled(False)
devs = self.joystickReader.getAvailableDevices()
if (len(devs) > 0):
self.device_discovery(devs)
def configInputDevice(self):
self.inputConfig = InputConfigDialogue(self.joystickReader)
self.inputConfig.show()
def _auto_reconnect_changed(self, checked):
self._auto_reconnect_enabled = checked
GuiConfig().set("auto_reconnect", checked)
logger.info("Auto reconnect enabled: %s", checked)
def doLogConfigDialogue(self):
self.logConfigDialogue.show()
def updateBatteryVoltage(self, timestamp, data, logconf):
self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
def connectionDone(self, linkURI):
self.setUIState(UIState.CONNECTED, linkURI)
GuiConfig().set("link_uri", linkURI)
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
self.cf.log.add_config(lg)
if lg.valid:
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
else:
logger.warning("Could not setup loggingblock!")
def _logging_error(self, log_conf, msg):
QMessageBox.about(
self, "Log error", "Error when starting log config"
" [%s]: %s" % (log_conf.name, msg))
def connectionLost(self, linkURI, msg):
if not self._auto_reconnect_enabled:
if (self.isActiveWindow()):
warningCaption = "Communication failure"
error = "Connection lost to %s: %s" % (linkURI, msg)
QMessageBox.critical(self, warningCaption, error)
self.setUIState(UIState.DISCONNECTED, linkURI)
else:
self.quickConnect()
def connectionFailed(self, linkURI, error):
if not self._auto_reconnect_enabled:
msg = "Failed to connect on %s: %s" % (linkURI, error)
warningCaption = "Communication failure"
QMessageBox.critical(self, warningCaption, msg)
self.setUIState(UIState.DISCONNECTED, linkURI)
else:
self.quickConnect()
def closeEvent(self, event):
self.hide()
self.cf.close_link()
GuiConfig().save_file()
def connectButtonClicked(self):
if (self.uiState == UIState.CONNECTED):
self.cf.close_link()
elif (self.uiState == UIState.CONNECTING):
self.cf.close_link()
self.setUIState(UIState.DISCONNECTED)
else:
self.connectDialogue.show()
def inputDeviceError(self, error):
self.cf.close_link()
QMessageBox.critical(self, "Input device error", error)
def _load_input_data(self):
self.joystickReader.stop_input()
# Populate combo box with available input device configurations
for c in ConfigManager().get_list_of_configs():
node = QAction(c,
self._menu_mappings,
checkable=True,
enabled=False)
node.toggled.connect(self._inputconfig_selected)
self.configGroup.addAction(node)
self._menu_mappings.addAction(node)
def _reload_configs(self, newConfigName):
# remove the old actions from the group and the menu
for action in self._menu_mappings.actions():
self.configGroup.removeAction(action)
self._menu_mappings.clear()
# reload the conf files, and populate the menu
self._load_input_data()
self._update_input(self._active_device, newConfigName)
def _update_input(self, device="", config=""):
self.joystickReader.stop_input()
self._active_config = str(config)
self._active_device = str(device)
GuiConfig().set("input_device", self._active_device)
GuiConfig().get("device_config_mapping")[
self._active_device] = self._active_config
self.joystickReader.start_input(self._active_device,
self._active_config)
# update the checked state of the menu items
for c in self._menu_mappings.actions():
c.setEnabled(True)
if c.text() == self._active_config:
c.setChecked(True)
for c in self._menu_devices.actions():
c.setEnabled(True)
if c.text() == self._active_device:
c.setChecked(True)
# update label
if device == "" and config == "":
self._statusbar_label.setText("No input device selected")
elif config == "":
self._statusbar_label.setText("Using [%s] - "
"No input config selected" %
(self._active_device))
else:
self._statusbar_label.setText(
"Using [%s] with config [%s]" %
(self._active_device, self._active_config))
def _inputdevice_selected(self, checked):
if (not checked):
return
self.joystickReader.stop_input()
sender = self.sender()
self._active_device = sender.text()
device_config_mapping = GuiConfig().get("device_config_mapping")
if (self._active_device in device_config_mapping.keys()):
self._current_input_config = device_config_mapping[str(
self._active_device)]
else:
self._current_input_config = self._menu_mappings.actions()[0].text(
)
GuiConfig().set("input_device", str(self._active_device))
for c in self._menu_mappings.actions():
if (c.text() == self._current_input_config):
c.setChecked(True)
self.joystickReader.start_input(str(sender.text()),
self._current_input_config)
self._statusbar_label.setText(
"Using [%s] with config [%s]" %
(self._active_device, self._current_input_config))
def _inputconfig_selected(self, checked):
if (not checked):
return
self._update_input(self._active_device, self.sender().text())
def device_discovery(self, devs):
group = QActionGroup(self._menu_devices, exclusive=True)
for d in devs:
node = QAction(d["name"], self._menu_devices, checkable=True)
node.toggled.connect(self._inputdevice_selected)
group.addAction(node)
self._menu_devices.addAction(node)
if (d["name"] == GuiConfig().get("input_device")):
self._active_device = d["name"]
if (len(self._active_device) == 0):
self._active_device = self._menu_devices.actions()[0].text()
device_config_mapping = GuiConfig().get("device_config_mapping")
if (device_config_mapping):
if (self._active_device in device_config_mapping.keys()):
self._current_input_config = device_config_mapping[str(
self._active_device)]
else:
self._current_input_config = self._menu_mappings.actions(
)[0].text()
else:
self._current_input_config = self._menu_mappings.actions()[0].text(
)
# Now we know what device to use and what mapping, trigger the events
# to change the menus and start the input
for c in self._menu_mappings.actions():
c.setEnabled(True)
if (c.text() == self._current_input_config):
c.setChecked(True)
for c in self._menu_devices.actions():
if (c.text() == self._active_device):
c.setChecked(True)
def quickConnect(self):
try:
self.cf.open_link(GuiConfig().get("link_uri"))
except KeyError:
self.cf.open_link("")
def _open_config_folder(self):
QDesktopServices.openUrl(
QUrl("file:///" + QDir.toNativeSeparators(sys.path[1])))
def closeAppRequest(self):
self.close()
sys.exit(0)
class Hover:
"""Uses vision tracking and PID-control to hover at a given altitude
1. Run camera capture in it's own thread; update altitude
2. Run hover loop in it's own thread; output altitude
"""
def __init__(self):
logger.info("{name} init()".format(name=self.__class__.__name__))
self.settings = shelve.open('/tmp/hover.settings')
self.initControl()
self.initCamera()
self.initTracking()
self.initHover()
self.initCF()
self.settings.close()
def initControl(self):
"""Setup control-flow variables"""
self.exit = False
def initCamera(self):
"""Setup camera variables
Will prompt the user for feedback. Capable of loading webcam or an
ip-camera streaming JPEG. Uses shelve to remember last entered
value
"""
# get settings
lastCamUri = self.settings.get('camUri')
lastCamRotate = self.settings.get('camRotate')
# prompt for camUri, camRotate
if lastCamUri:
_input = raw_input("Specify camera: 'cam' for webcam, " +
"an ip of network camera or <ENTER> for the " +
"previous value of '{lastCamUri}':\n"
.format(lastCamUri=lastCamUri))
if not _input:
self.camUri = lastCamUri
else:
self.camUri = _input
else:
_input = raw_input("Specify camera: 'cam'/<ENTER> for webcam or " +
"an ip of network camera:\n")
if not _input:
self.camUri = _input
else:
self.camUri = 'cam'
logger.info("CamUri = '{camUri}'".format(camUri=self.camUri))
if lastCamRotate:
_input = raw_input("Specify camera rotation or <ENTER> for the " +
"previous value of '{lastCamRotate}':\n"
.format(lastCamRotate=lastCamRotate))
if _input:
self.camRotate = int(_input)
else:
self.camRotate = lastCamRotate
else:
_input = raw_input("Specify camera rotation or <ENTER> for " +
"no rotation':\n")
if _input:
self.camRotate = int(_input)
else:
self.camRotate = 0
logger.info("CamRotate = '{camRotate}'"\
.format(camRotate=self.camRotate))
# save settings
self.settings['camUri'] = self.camUri
self.settings['camRotate'] = self.camRotate
# setup camera
if self.camUri == "cam":
self.cam = Camera()
elif '.' not in self.camUri:
self.cam = JpegStreamCamera("http://192.168.1.{ip}:8080/video"
.format(ip=self.camUri))
else:
self.cam = JpegStreamCamera("http://{ip}:8080/video"
.format(ip=self.camUri))
# additional values -- for now, just hard coded
self.camRes = (800,600)
logger.info("Camera resolution={res}".format(res=self.camRes))
def initTracking(self):
"""Setup tracking variables
Will prompt the user for tracking variables. Uses shelve to remember
last entered value
"""
# get last values
lastTrackingColor = self.settings.get('trackingColor')
# prompt for override
if lastTrackingColor:
_input = raw_input("Specify tracking color as (R,G,B)" +
"or <ENTER> for previous value " +
"{lastTrackingColor}:\n"
.format(lastTrackingColor=lastTrackingColor))
if _input:
self.trackingColor = make_tuple(_input)
else:
self.trackingColor = lastTrackingColor
else:
_input = raw_input("Specify tracking color as (R,G,B)" +
"or <ENTER> for default of BLUE:\n")
if _input:
self.trackingColor = make_tuple(_input)
else:
self.trackingColor = Color.BLUE
# save settings
self.settings['trackingColor'] = self.trackingColor
# additional values
self.trackingBlobMin = 5
self.trackingBlobMax = 1000
self.x = -1
self.y = -1
self.target = (300,400)
logger.info(("Tracking color={color}; min={min}; max={max}; " +
"target={target}")
.format(color=self.trackingColor,
min=self.trackingBlobMin,
max=self.trackingBlobMax,
target=self.target))
self.trackingFrameQ = Queue()
def initHover(self):
self.hoverFrames = []
self.eSum = 0
def initCF(self):
self.cfConnected = False
self.cf = None
self.cfUri = None
logger.debug("Initializing Drivers; Debug=FALSE")
InitDrivers(enable_debug_driver=False)
logger.info("Scanning")
available = Scan()
logger.info("Found:")
for cf in available:
logger.debug(cf[0])
if len(available) > 0:
self.cfUri = available[0][0]
self.cf = Crazyflie()
self.cf.connected.add_callback(self.cfOnConnected)
self.cf.disconnected.add_callback(self.cfOnDisconnected)
self.cf.connection_failed.add_callback(self.cfOnFailed)
self.cf.connection_lost.add_callback(self.cfOnLost)
logger.info("Crazyflie @{uri} Initialized".format(uri=self.cfUri))
else:
logger.info("No Crazyflies found")
def cfOnConnected(self, uri):
logger.info("{func} {uri}".format(func=inspect.stack()[0][3], uri=uri))
self.cfConnected = True
def cfOnDisconnected(self, uri):
logger.info("{func} {uri}".format(func=inspect.stack()[0][3], uri=uri))
self.cfConnected = False
def cfOnFailed(self, uri):
logger.info("{func} {uri}".format(func=inspect.stack()[0][3], uri=uri))
def cfOnLost(self, uri, msg):
logger.info("{func} {uri} {msg}".format(func=inspect.stack()[0][3],
uri=uri, msg=msg))
def visionLoop(self):
while not self.exit:
# acquire image
img = self.cam.getImage()
# exit if we've got nothing
if img is None:
break
# adjust image
if self.camRotate != 0:
img.rotate(self.camrotate)
'''
img = img.resize(self.camRes[0], self.camRes[1])
img = img.rotate90()
'''
# blob search
colorDiff = img - img.colorDistance(self.trackingColor)
blobs = colorDiff.findBlobs(-1, self.trackingBlobMin,
self.trackingBlobMax)
'''
blobs = img.findBlobs((255,60,60), self.trackingBlobMin,
self.trackingBlobMax)
'''
# blob find
if blobs is not None:
self.x = blobs[-1].x
self.y = blobs[-1].y
# blob show
if blobs is not None:
# roi = region of interest
roiLayer = DrawingLayer((img.width, img.height))
# draw all blobs
for blob in blobs:
blob.draw(layer=roiLayer)
# draw a circle around the main blob
roiLayer.circle((self.x,self.y), 50, Color.RED, 2)
# apply roi to img
img.addDrawingLayer(roiLayer)
img = img.applyLayers()
img.show()
# fps
now = datetime.utcnow()
self.trackingFrameQ.put(now)
if self.trackingFrameQ.qsize() < 30:
fps = 0.0
else:
fps = 30.0/(now - self.trackingFrameQ.get()).total_seconds()
# logging
logger.debug("{func} ({x},{y}) {fps:5.2f}"
.format(func=inspect.stack()[0][3],
x=self.x, y=self.y, fps=fps))
# loop has ened
logger.debug("{func} stopped.".format(func=inspect.stack()[0][3]))
def hoverLoop(self):
while not self.exit:
# hover throttled to camera frame rate
sleep(1.0/30.0)
# fps
now = datetime.utcnow()
self.hoverFrames.append(now)
if len(self.hoverFrames) < 30:
fps = 0.0
else:
fps = 30.0/(now - self.hoverFrames[-30]).total_seconds()
# pid variables
kp = (3000.0/400.0)
ki = 0.18
# determine immediate error e
e = self.y - self.target[1]
# determine accumulated errors eSum
if len(self.hoverFrames) > 1:
dt = (now - self.hoverFrames[-2]).total_seconds()
else:
dt = 0
self.eSum = self.eSum + e * dt
# calculate thrust
hoverThrust = 37000
thrust = hoverThrust + (kp * e) + (ki * self.eSum)
# set the throttle
self.setThrust(thrust)
# logging
logger.debug(("{func} e={e}, dt={dt:0.4f}, eSum={eSum:0.4f}, " +
"thrust={thrust}, [{fps:5.2f}]")
.format(func=inspect.stack()[0][3],
e=e, dt=dt, eSum=self.eSum, thrust=thrust,
fps=fps))
# loop has ened
logger.debug("{func} stopped.".format(func=inspect.stack()[0][3]))
def setThrust(self, thrust):
""" sets thrust - but if control has exited, will kill thrust """
if self.exit:
thrust = 0
if self.cf is not None:
self.cf.commander.send_setpoint(0,0,0, thrust)
def cfOpenLink(self):
if self.cf is not None:
self.cf.open_link(self.cfUri)
logger.info("Linked opened to {uri}".format(uri=self.cfUri))
def start(self):
logger.info("Starting VisionLoop")
threading.Thread(target=self.visionLoop).start()
logger.info("Starting HoverLoop")
threading.Thread(target=self.hoverLoop).start()
logger.info("Opening Crazyflie link")
self.cfOpenLink()
raw_input("Press enter to stop")
self.stop()
def stop(self):
# kill our loops
self.exit = True
# explicitly kill thrust
self.setThrust(0)
# kill crazyflie
if self.cf and self.cfConnected:
self.cf.close_link()
