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, 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 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()
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 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 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 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 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 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 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 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("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 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 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 _ramp_motors(self):
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)
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 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
class Main:
def __init__(self):
self.thrust = 25000
self.pitch = -4
self.roll = 0
self.yaw = 0
self.stopping = False
self.jump = 0
self.backward = 0
self.forward = 0
Thread(target=self.gui).start()
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/7/250K")
self.crazyflie.open_link("radio://0/10/250K")
#self.crazyflie.open_link("radio://0/6/1M")
self.crazyflie.connectSetupFinished.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 gui(self):
print "bingo"
while self.stopping == False:
#nb = _GetchUnix()
nb = sys.stdin.read(1)
if nb == 'x':
self.stopping = True
if nb == 'r':
self.thrust = self.thrust + 1000
if nb == 'f':
self.thrust = self.thrust - 1000
if nb == 'y':
self.backward = 0
self.forward = 3
if nb == 'h':
self.backward = 3
self.forward = 0
if nb == '3':
self.thrust = 35000
if nb == '4':
self.thrust = 40000
if nb == 'e':
self.yaw = self.yaw + 1
if nb == 'q':
self.yaw = self.yaw - 1
if nb == 'd':
self.roll = self.roll + 2
if nb == 'a':
self.roll = self.roll - 2
if nb == 'w':
self.pitch = self.pitch - 2
if nb == 's':
self.pitch = self.pitch + 2
if nb == 'z':
self.jump = 2
sys.stdout.write("thrust=")
print self.thrust
sys.stdout.write("yaw=")
print self.yaw
sys.stdout.write("pitch=")
print self.pitch
sys.stdout.write("roll=")
print self.roll
def pulse_command(self):
while self.stopping == False:
lthrust = self.thrust
lpitch = self.pitch
if self.jump > 0:
lthrust = self.thrust + 23000
self.jump = self.jump - 1
if self.forward > 0:
lpitch = self.pitch + 4
self.forward = self.forward - 1
if self.backward > 0:
lpitch = self.pitch - 4
self.backward = self.backward - 1
self.crazyflie.commander.send_setpoint(self.roll, lpitch, self.yaw,
lthrust)
time.sleep(0.15)
self.crazyflie.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
self.crazyflie.close_link()
class Drone2:
VELOCITY = 0.2
RATE = 360.0 / 5
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)
# self._thread = None
self._thread = _SetPointThread(self._cf)
self._is_flying = False
self.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."""
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._oscillate).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))
self.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))
self.connected = False
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.connected = False
#########################This is From the Motion Commander Library #########
def take_off(self, height=None, velocity=VELOCITY):
"""
Takes off, that is starts the motors, goes straigt up and hovers.
Do not call this function if you use the with keyword. Take off is
done automatically when the context is created.
:param height: the height (meters) to hover at. None uses the default
height set when constructed.
:param velocity: the velocity (meters/second) when taking off
:return:
"""
if self._is_flying:
raise Exception('Already flying')
if not self._cf.is_connected():
raise Exception('Crazyflie is not connected')
self._is_flying = True
self._reset_position_estimator()
self._thread = _SetPointThread(self._cf)
self._thread.start()
if height is None:
height = self.default_height
self.up(height, velocity)
def land(self, velocity=VELOCITY):
"""
Go straight down and turn off the motors.
Do not call this function if you use the with keyword. Landing is
done automatically when the context goes out of scope.
:param velocity: The velocity (meters/second) when going down
:return:
"""
if self._is_flying:
self.down(self._thread.get_height(), velocity)
self._thread.stop()
self._thread = None
def _update_z_in_setpoint(self):
self._hover_setpoint[self.ABS_Z_INDEX] = self._current_z()
def _current_z(self):
now = time.time()
return self._z_base + self._z_velocity * (now - self._z_base_time)
def __enter__(self):
self.take_off()
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self.land()
def left(self, distance_m, velocity=VELOCITY):
"""
Go left
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(0.0, distance_m, 0.0, velocity)
def right(self, distance_m, velocity=VELOCITY):
"""
Go right
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(0.0, -distance_m, 0.0, velocity)
def forward(self, distance_m, velocity=VELOCITY):
"""
Go forward
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(distance_m, 0.0, 0.0, velocity)
def back(self, distance_m, velocity=VELOCITY):
"""
Go backwards
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(-distance_m, 0.0, 0.0, velocity)
def up(self, distance_m, velocity=VELOCITY):
"""
Go up
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(0.0, 0.0, distance_m, velocity)
def down(self, distance_m, velocity=VELOCITY):
"""
Go down
:param distance_m: the distance to travel (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
self.move_distance(0.0, 0.0, -distance_m, velocity)
def move_distance(self,
distance_x_m,
distance_y_m,
distance_z_m,
velocity=VELOCITY):
"""
Move in a straight line.
positive X is forward
positive Y is left
positive Z is up
:param distance_x_m: The distance to travel along the X-axis (meters)
:param distance_y_m: The distance to travel along the Y-axis (meters)
:param distance_z_m: The distance to travel along the Z-axis (meters)
:param velocity: the velocity of the motion (meters/second)
:return:
"""
distance = math.sqrt(distance_x_m * distance_x_m +
distance_y_m * distance_y_m +
distance_z_m * distance_z_m)
flight_time = distance / velocity
velocity_x = velocity * distance_x_m / distance
velocity_y = velocity * distance_y_m / distance
velocity_z = velocity * distance_z_m / distance
self.start_linear_motion(velocity_x, velocity_y, velocity_z)
time.sleep(flight_time)
self.stop()
# Velocity based primitives
def start_left(self, velocity=VELOCITY):
"""
Start moving left. This function returns immediately.
:param velocity: The velocity of the motion (meters/second)
:return:
"""
self.start_linear_motion(0.0, velocity, 0.0)
def start_right(self, velocity=VELOCITY):
"""
Start moving right. This function returns immediately.
:param velocity: The velocity of the motion (meters/second)
:return:
"""
self.start_linear_motion(0.0, -velocity, 0.0)
def start_forward(self, velocity=VELOCITY):
"""
Start moving forward. This function returns immediately.
:param velocity: The velocity of the motion (meters/second)
:return:
"""
self.start_linear_motion(velocity, 0.0, 0.0)
def start_back(self, velocity=VELOCITY):
"""
Start moving backwards. This function returns immediately.
:param velocity: The velocity of the motion (meters/second)
:return:
"""
self.start_linear_motion(-velocity, 0.0, 0.0)
def start_up(self, velocity=VELOCITY):
"""
Start moving up. This function returns immediately.
:param velocity: The velocity of the motion (meters/second)
:return:
"""
self.start_linear_motion(0.0, 0.0, velocity)
def start_down(self, velocity=VELOCITY):
"""
Start moving down. This function returns immediately.
:param velocity: The velocity of the motion (meters/second)
:return:
"""
self.start_linear_motion(0.0, 0.0, -velocity)
def stop(self):
"""
Stop any motion and hover.
:return:
"""
self._set_vel_setpoint(0.0, 0.0, 0.0, 0.0)
def start_turn_left(self, rate=RATE):
"""
Start turning left. This function returns immediately.
:param rate: The angular rate (degrees/second)
:return:
"""
self._set_vel_setpoint(0.0, 0.0, 0.0, -rate)
def start_turn_right(self, rate=RATE):
"""
Start turning right. This function returns immediately.
:param rate: The angular rate (degrees/second)
:return:
"""
self._set_vel_setpoint(0.0, 0.0, 0.0, rate)
def start_circle_left(self, radius_m, velocity=VELOCITY):
"""
Start a circular motion to the left. This function returns immediately.
:param radius_m: The radius of the circle (meters)
:param velocity: The velocity of the motion (meters/second)
:return:
"""
circumference = 2 * radius_m * math.pi
rate = 360.0 * velocity / circumference
self._set_vel_setpoint(velocity, 0.0, 0.0, -rate)
def start_circle_right(self, radius_m, velocity=VELOCITY):
"""
Start a circular motion to the right. This function returns immediately
:param radius_m: The radius of the circle (meters)
:param velocity: The velocity of the motion (meters/second)
:return:
"""
circumference = 2 * radius_m * math.pi
rate = 360.0 * velocity / circumference
self._set_vel_setpoint(velocity, 0.0, 0.0, rate)
def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m):
"""
Start a linear motion. This function returns immediately.
positive X is forward
positive Y is left
positive Z is up
:param velocity_x_m: The velocity along the X-axis (meters/second)
:param velocity_y_m: The velocity along the Y-axis (meters/second)
:param velocity_z_m: The velocity along the Z-axis (meters/second)
:return:
"""
self._set_vel_setpoint(velocity_x_m, velocity_y_m, velocity_z_m, 0.0)
def _set_vel_setpoint(self, velocity_x, velocity_y, velocity_z, rate_yaw):
if not self._is_flying:
raise Exception('Can not move on the ground. Take off first!')
self._thread.set_vel_setpoint(velocity_x, velocity_y, velocity_z,
rate_yaw)
def _reset_position_estimator(self):
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
####### Actually getting it to oscillate ##########################
def _oscillate(self):
self.take_off(0.35, 0.6)
#self.move_distance(0,0,0.25,0.6)
amplitude = 0.25
peak = 2 * amplitude
global t
while True:
if keyboard.is_pressed('w'):
oscillate = True
while oscillate == True:
self.move_distance(0, 0, peak, v_inp2)
t = t + tau2 / 2
disp = np.exp(-z2 * wn2 * t) * amplitude
peak = amplitude + disp
amplitude = disp
print(peak)
time.sleep(0.2)
# self.start_circle_right(0.43,0.3)
self.move_distance(0, 0, -peak, v_inp2)
self.start_circle_right(0.43, 0.3)
t = t + tau2 / 2
disp = np.exp(-z2 * wn2 * t) * amplitude
peak = amplitude + disp
amplitude = disp
print(peak)
time.sleep(0.2)
# self.move_distance(0,0,-peak,v_inp2)
# while True:
if keyboard.is_pressed('p') or peak < 2.18 * 10**(-7):
oscillate = False
# break
break
while True:
if keyboard.is_pressed('s'):
oscillate == False
self.land(velocity=0.6)
break
self._cf.close_link()
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 - applying stylesheet")
tcss = """
QProgressBar {
border: 1px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: """ + COLOR_BLUE + """;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite - skinny bar stylesheet not applied")
######################################################
self.cf = Crazyflie(ro_cache=None,
rw_cache=cfclient.config_path + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=Config()
.get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
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)
# Hide the 'File' menu on OS X, since its only item, 'Exit', gets
# merged into the application menu.
if sys.platform == 'darwin':
self.menuFile.menuAction().setVisible(False)
# Connect UI signals
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self.address.setValue(0xE7E7E7E7E7)
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(self._disconnected)
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(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
self.linkQualityBar.setTextVisible(False)
self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# 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 is not 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))
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node,)
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node,)
self._all_role_menus += ({"muxmenu": node,
"rolemenu": sub_node},)
node.setData((m, mux_subnodes))
self._mapping_support = True
def interfaceChanged(self, interface):
if interface == INTERFACE_PROMPT_TEXT:
self._selected_interface = None
else:
self._selected_interface = interface
self._update_ui_state()
def foundInterfaces(self, interfaces):
selected_interface = self._selected_interface
self.interfaceCombo.clear()
self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT)
formatted_interfaces = []
for i in interfaces:
if len(i[1]) > 0:
interface = "%s - %s" % (i[0], i[1])
else:
interface = i[0]
formatted_interfaces.append(interface)
self.interfaceCombo.addItems(formatted_interfaces)
if self._initial_scan:
self._initial_scan = False
try:
selected_interface = Config().get("link_uri")
except KeyError:
pass
newIndex = 0
if selected_interface is not None:
try:
newIndex = formatted_interfaces.index(selected_interface) + 1
except ValueError:
pass
self.interfaceCombo.setCurrentIndex(newIndex)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _update_ui_state(self):
if self.uiState == UIState.DISCONNECTED:
self.setWindowTitle("Not connected")
canConnect = self._selected_interface is not None
self.menuItemConnect.setText("Connect to Crazyflie")
self.menuItemConnect.setEnabled(canConnect)
self.connectButton.setText("Connect")
self.connectButton.setToolTip(
"Connect to the Crazyflie on the selected interface")
self.connectButton.setEnabled(canConnect)
self.scanButton.setText("Scan")
self.scanButton.setEnabled(True)
self.address.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)
self.interfaceCombo.setEnabled(True)
elif self.uiState == UIState.CONNECTED:
s = "Connected on %s" % self._selected_interface
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Disconnect")
self.connectButton.setToolTip("Disconnect from the Crazyflie")
self.scanButton.setEnabled(False)
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)
elif self.uiState == UIState.CONNECTING:
s = "Connecting to {} ...".format(self._selected_interface)
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Cancel")
self.connectButton.setToolTip("Cancel connecting to the Crazyflie")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.setEnabled(False)
elif self.uiState == UIState.SCANNING:
self.setWindowTitle("Scanning ...")
self.connectButton.setText("Connect")
self.menuItemConnect.setEnabled(False)
self.connectButton.setText("Connect")
self.connectButton.setEnabled(False)
self.scanButton.setText("Scanning...")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.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_battery(self, timestamp, data, logconf):
self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
color = COLOR_BLUE
# TODO firmware reports fully-charged state as 'Battery',
# rather than 'Charged'
if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]:
color = COLOR_GREEN
elif data["pm.state"] == BatteryStates.LOW_POWER:
color = COLOR_RED
self.batteryBar.setStyleSheet(progressbar_stylesheet(color))
def _connected(self):
self.uiState = UIState.CONNECTED
self._update_ui_state()
Config().set("link_uri", str(self._selected_interface))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
lg.add_variable("pm.state", "int8_t")
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))
mem = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)[0]
mem.write_data(self._led_write_done)
# self._led_write_test = 0
# mem.leds[self._led_write_test] = [10, 20, 30]
# mem.write_data(self._led_write_done)
def _disconnected(self):
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _connection_initiated(self):
self.uiState = UIState.CONNECTING
self._update_ui_state()
def _led_write_done(self, mem, addr):
logger.info("LED write done callback")
# self._led_write_test += 1
# mem.leds[self._led_write_test] = [10, 20, 30]
# mem.write_data(self._led_write_done)
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.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._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.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._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.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self.cf.open_link(self._selected_interface)
def _scan(self):
self.uiState = UIState.SCANNING
self._update_ui_state()
self.scanner.scanSignal.emit(self.address.value())
def _display_input_device_error(self, error):
self.cf.close_link()
QMessageBox.critical(self, "Input device error", error)
def _mux_selected(self, checked):
"""Called when a new mux is selected. The menu item contains a
reference to the raw mux object as well as to the associated device
sub-nodes"""
if not checked:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(False)
else:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(True)
self.joystickReader.set_mux(mux=mux)
# Go though the tree and select devices/mapping that was
# selected before it was disabled.
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
dev_node.toggled.emit(True)
self._update_input_device_footer()
def _get_dev_status(self, device):
msg = "{}".format(device.name)
if device.supports_mapping:
map_name = "N/A"
if device.input_map:
map_name = device.input_map_name
msg += " ({})".format(map_name)
return msg
def _update_input_device_footer(self):
"""Update the footer in the bottom of the UI with status for the
input device and its mapping"""
msg = ""
if len(self.joystickReader.available_devices()) > 0:
mux = self.joystickReader._selected_mux
msg = "Using {} mux with ".format(mux.name)
for key in list(mux._devs.keys())[:-1]:
if mux._devs[key]:
msg += "{}, ".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A, "
# Last item
key = list(mux._devs.keys())[-1]
if mux._devs[key]:
msg += "{}".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A"
else:
msg = "No input device found"
self._statusbar_label.setText(msg)
def _inputdevice_selected(self, checked):
"""Called when a new input device has been selected from the menu. The
data in the menu object is the associated map menu (directly under the
item in the menu) and the raw device"""
(map_menu, device, mux_menu) = self.sender().data()
if not checked:
if map_menu:
map_menu.setEnabled(False)
# Do not close the device, since we don't know exactly
# how many devices the mux can have open. When selecting a
# new mux the old one will take care of this.
else:
if map_menu:
map_menu.setEnabled(True)
(mux, sub_nodes) = mux_menu.data()
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
if device.id == dev_node.data()[1].id \
and dev_node is not self.sender():
dev_node.setChecked(False)
role_in_mux = str(self.sender().parent().title()).strip()
logger.info("Role of {} is {}".format(device.name,
role_in_mux))
Config().set("input_device", str(device.name))
self._mapping_support = self.joystickReader.start_input(
device.name,
role_in_mux)
self._update_input_device_footer()
def _inputconfig_selected(self, checked):
"""Called when a new configuration has been selected from the menu. The
data in the menu object is a referance to the device QAction in parent
menu. This contains a referance to the raw device."""
if not checked:
return
selected_mapping = str(self.sender().text())
device = self.sender().data().data()[1]
self.joystickReader.set_input_map(device.name, selected_mapping)
self._update_input_device_footer()
def device_discovery(self, devs):
"""Called when new devices have been added"""
for menu in self._all_role_menus:
role_menu = menu["rolemenu"]
mux_menu = menu["muxmenu"]
dev_group = QActionGroup(role_menu, exclusive=True)
for d in devs:
dev_node = QAction(d.name, role_menu, checkable=True,
enabled=True)
role_menu.addAction(dev_node)
dev_group.addAction(dev_node)
dev_node.toggled.connect(self._inputdevice_selected)
map_node = None
if d.supports_mapping:
map_node = QMenu(" Input map", role_menu, enabled=False)
map_group = QActionGroup(role_menu, exclusive=True)
# Connect device node to map node for easy
# enabling/disabling when selection changes and device
# to easily enable it
dev_node.setData((map_node, d))
for c in ConfigManager().get_list_of_configs():
node = QAction(c, map_node, checkable=True,
enabled=True)
node.toggled.connect(self._inputconfig_selected)
map_node.addAction(node)
# Connect all the map nodes back to the device
# action node where we can access the raw device
node.setData(dev_node)
map_group.addAction(node)
# If this device hasn't been found before, then
# select the default mapping for it.
if d not in self._available_devices:
last_map = Config().get("device_config_mapping")
if d.name in last_map and last_map[d.name] == c:
node.setChecked(True)
role_menu.addMenu(map_node)
dev_node.setData((map_node, d, mux_menu))
# Update the list of what devices we found
# to avoid selecting default mapping for all devices when
# a new one is inserted
self._available_devices = ()
for d in devs:
self._available_devices += (d,)
# Only enable MUX nodes if we have enough devies to cover
# the roles
for mux_node in self._all_mux_nodes:
(mux, sub_nodes) = mux_node.data()
if len(mux.supported_roles()) <= len(self._available_devices):
mux_node.setEnabled(True)
# TODO: Currently only supports selecting default mux
if self._all_mux_nodes[0].isEnabled():
self._all_mux_nodes[0].setChecked(True)
# If the previous length of the available devies was 0, then select
# the default on. If that's not available then select the first
# on in the list.
# TODO: This will only work for the "Normal" mux so this will be
# selected by default
if Config().get("input_device") in [d.name for d in devs]:
for dev_menu in self._all_role_menus[0]["rolemenu"].actions():
if dev_menu.text() == Config().get("input_device"):
dev_menu.setChecked(True)
else:
# Select the first device in the first mux (will always be "Normal"
# mux)
self._all_role_menus[0]["rolemenu"].actions()[0].setChecked(True)
logger.info("Select first device")
self._update_input_device_footer()
def _open_config_folder(self):
QDesktopServices.openUrl(
QUrl("file:///" +
QDir.toNativeSeparators(cfclient.config_path)))
def closeAppRequest(self):
self.close()
sys.exit(0)
class OWExample:
"""
Simple example listing the 1-wire memories found and lists its contents.
"""
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
self._mems_to_update = 0
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_1W)
self._mems_to_update = len(mems)
print('Found {} 1-wire memories'.format(len(mems)))
for m in mems:
print('Updating id={}'.format(m.id))
m.update(self._data_updated)
def _data_updated(self, mem):
print('Updated id={}'.format(mem.id))
print('\tAddr : {}'.format(mem.addr))
print('\tType : {}'.format(mem.type))
print('\tSize : {}'.format(mem.size))
print('\tValid : {}'.format(mem.valid))
print('\tName : {}'.format(mem.name))
print('\tVID : 0x{:02X}'.format(mem.vid))
print('\tPID : 0x{:02X}'.format(mem.pid))
print('\tPins : 0x{:02X}'.format(mem.pins))
print('\tElements : ')
for key in mem.elements:
print('\t\t{}={}'.format(key, mem.elements[key]))
self._mems_to_update -= 1
if self._mems_to_update == 0:
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 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 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 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 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("Proximity sensor", 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
t = Timer(10, self._cf.close_link)
t.start()
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
#Thread(target=self._ramp_motors).start()
def _log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global myprox
global ref_baro
myprox = data["adc.vProx"]
ref_baro = data["baro.aslLong"]
#long = float(data["adc.vProx"])
#if myprox >= 2:
# print(myprox, ref_baro)
#else:
# print("Sensor range low", test)
Thread(target=self._ramp_motors).start()
#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 _ramp_motors(self):
thrust_mult = 1
thrust_step = 100
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
print(myprox, 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
#cf.commander.send_setpoint(0,0,0,initial_thrust);
#time.sleep(0.5);
#cf.param.set_value("flightmode.althold","True");
# while some_condition:
# cf.commander.send_setpoint(0,0,0,32767);
# time.sleep(0.01);
time.sleep(0.1)
self._cf.close_link()
class Drone:
"""Represents a CrazyFlie drone."""
def __init__(self,
drone_id: str,
arena: Arena,
radio_id: int = 0,
channel: int = 80,
address: str = "E7E7E7E7E7",
data_rate: str = "2M"):
""" Initializes the drone with the given uri."""
# Initialize public variables
self.id: str = drone_id
self.var_x: float = 0
self.var_y: float = 0
self.var_z: float = 0
self.pos_x: float = 0
self.pos_y: float = 0
self.pos_z: float = 0
self.pitch: float = 0
self.roll: float = 0
self.yaw: float = 0
self.battery_voltage: float = 0
self.is_connected: bool = False
self.status: DroneState = DroneState.OFFLINE
self.link_uri: str = "radio://" + str(radio_id) + "/" + str(
channel) + "/" + data_rate + "/" + address
# Initialize limits
self._max_velocity: float = 1.0
self._min_duration: float = 1.0
self._max_yaw_rotations: float = 1.0
self._arena = arena
# Event to asynchronously wait for the connection
self._connect_event = threading.Event()
# Initialize the crazyflie
self._cf = Crazyflie(rw_cache='./cache')
# Initialize the callbacks
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)
# Initialize events
self.drone_lost = Caller()
# Define the log configuration
self._log_config_1 = LogConfig(name='DroneLog_1', period_in_ms=500)
self._log_config_1.add_variable('kalman.varPX', 'float')
self._log_config_1.add_variable('kalman.varPY', 'float')
self._log_config_1.add_variable('kalman.varPZ', 'float')
self._log_config_1.add_variable('pm.vbat', 'float')
self._log_config_2 = LogConfig(name='DroneLog_2', period_in_ms=100)
self._log_config_2.add_variable('kalman.stateX', 'float')
self._log_config_2.add_variable('kalman.stateY', 'float')
self._log_config_2.add_variable('kalman.stateZ', 'float')
self._log_config_3 = LogConfig(name='DroneLog_3', period_in_ms=500)
self._log_config_3.add_variable('stabilizer.pitch', 'float')
self._log_config_3.add_variable('stabilizer.roll', 'float')
self._log_config_3.add_variable('stabilizer.yaw', 'float')
def connect(self, synchronous: bool = False):
"""Connects to the Crazyflie."""
self._connect_crazyflie()
if synchronous:
self._connect_event.wait()
def disconnect(self):
"""Disconnects from the Crazyflie and stops all logging."""
self._disconnect_crazyflie()
def enable_high_level_commander(self):
"""Enables the drones high level commander."""
self._cf.param.set_value('commander.enHighLevel', '1')
time.sleep(0.1)
def disable_motion_tracking(self):
"""Disables to motion control (x/y) from the flow-deck."""
self._cf.param.set_value('motion.disable', '1')
time.sleep(0.1)
def get_status(self) -> str:
"""Gets various information of the drone."""
return {
"id":
self.id,
"var_x":
self.var_x,
"var_y":
self.var_y,
"var_z":
self.var_z,
"x":
self._arena.transform_x_inverse(self.pos_x),
"y":
self._arena.transform_y_inverse(self.pos_y),
"z":
self.pos_z,
"pitch":
self.pitch,
"roll":
self.roll,
"yaw":
self.yaw,
"status":
self.status.name,
"battery_voltage":
self.battery_voltage,
"battery_percentage:":
(self.battery_voltage - 3.4) / (4.18 - 3.4) * 100
}
def reset_estimator(self) -> bool:
"""Resets the position estimates."""
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2.0)
# TODO: wait_for_position_estimator(cf)
return True
def takeoff(self,
absolute_height: float,
velocity: float,
synchronous: bool = False) -> float:
absolute_height = self._sanitize_z(absolute_height, False)
self.reset_estimator()
duration = self._convert_velocity_to_time(absolute_height, velocity)
self._cf.high_level_commander.takeoff(absolute_height, duration)
self.status = DroneState.STARTING
if synchronous:
time.sleep(duration)
return {"duration": duration, "target_z": absolute_height}
def land(self,
absolute_height: float,
velocity: float,
synchronous: bool = False) -> float:
absolute_height = self._sanitize_z(absolute_height, False)
duration = self._convert_velocity_to_time(absolute_height, velocity)
self._cf.high_level_commander.land(absolute_height, duration)
self.status = DroneState.LANDING
if synchronous:
time.sleep(duration)
return {"duration": duration, "target_z": absolute_height}
def go_to(self,
x: float,
y: float,
z: float,
yaw: float,
velocity: float,
relative: bool = False,
synchronous: bool = False) -> float:
x = self._sanitize_x(x, relative)
y = self._sanitize_y(y, relative)
z = self._sanitize_z(z, relative)
yaw = self._sanitize_yaw(yaw)
distance = self._calculate_distance(x, y, z, relative)
duration = self._convert_velocity_to_time(distance, velocity)
self._cf.high_level_commander.go_to(x, y, z, yaw, duration, relative)
self.status = DroneState.NAVIGATING
if synchronous:
time.sleep(duration)
return {
"duration": duration,
"target_x": self._arena.transform_x_inverse(x),
"target_y": self._arena.transform_y_inverse(y),
"target_z": z,
"target_yaw": yaw,
"relative": relative
}
def stop(self):
self._cf.high_level_commander.stop()
self.status = DroneState.IDLE
def _connect_crazyflie(self):
print('Connecting to %s' % self.link_uri)
self._cf.open_link(self.link_uri)
def _disconnect_crazyflie(self):
print('Disconnecting from %s' % self.link_uri)
# Stop the loggers
self._log_config_1.stop()
self._log_config_2.stop()
self._log_config_3.stop()
# Shutdown the rotors
self._shutdown()
# Disconnect
self._cf.close_link()
def _connected(self, link_uri):
"""This callback is called when the Crazyflie has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
# Setup parameters
self.disable_motion_tracking()
# Add the logger
self._cf.log.add_config(self._log_config_1)
self._cf.log.add_config(self._log_config_2)
self._cf.log.add_config(self._log_config_3)
# This callback will receive the data
self._log_config_1.data_received_cb.add_callback(
self._log_config_1_data)
self._log_config_2.data_received_cb.add_callback(
self._log_config_2_data)
self._log_config_3.data_received_cb.add_callback(
self._log_config_3_data)
# This callback will be called on errors
self._log_config_1.error_cb.add_callback(self._log_config_error)
self._log_config_2.error_cb.add_callback(self._log_config_error)
self._log_config_3.error_cb.add_callback(self._log_config_error)
# Start the logging
self._log_config_1.start()
self._log_config_2.start()
self._log_config_3.start()
# Set the connected event
self._connect_event.set()
self.is_connected = True
self.status = DroneState.IDLE
def _connection_failed(self, link_uri, msg):
"""Callback when the initial connection fails."""
print('Connection to %s failed: %s' % (link_uri, msg))
# Set the connected event
self._connect_event.set()
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected."""
print('Disconnected from %s' % link_uri)
self.is_connected = False
self.status = DroneState.OFFLINE
def _connection_lost(self, link_uri, msg):
"""Callback when the connection is lost after a connection has been made."""
print('Connection to %s lost: %s' % (link_uri, msg))
self.drone_lost.call(self)
self._connect_event.set()
self.is_connected = False
self.status = DroneState.OFFLINE
def _log_config_error(self, logconf, msg):
"""Callback from the log API when an error occurs."""
print('Error when logging %s: %s' % (logconf.name, msg))
def _log_config_1_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives."""
self.var_x = data['kalman.varPX']
self.var_y = data['kalman.varPY']
self.var_z = data['kalman.varPZ']
self.battery_voltage = data['pm.vbat']
def _log_config_2_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives."""
self.pos_x = data['kalman.stateX']
self.pos_y = data['kalman.stateY']
self.pos_z = data['kalman.stateZ']
def _log_config_3_data(self, timestamp, data, logconf):
self.pitch = data['stabilizer.pitch']
self.roll = data['stabilizer.roll']
self.yaw = data['stabilizer.yaw']
def _unlock(self):
# Unlock startup thrust protection (only needed for low lewel commands)
self._cf.commander.send_setpoint(0, 0, 0, 0)
def _shutdown(self):
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)
def _keep_setpoint(self, roll, pitch, yawrate, thrust, keeptime):
"""Keeps the drone at the given setpoint for the given amount of time."""
while keeptime > 0:
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
keeptime -= 0.1
time.sleep(0.1)
def _convert_velocity_to_time(self, distance: float,
velocity: float) -> float:
"""Converts a distance and a velocity to a time."""
duration = distance / self._sanitize_velocity(velocity)
return self._sanitize_duration(duration)
def _calculate_distance(self,
x: float,
y: float,
z: float,
relative: bool = False) -> float:
"""Calculates the distance from the drone or the zero position (relative) to a given point in space."""
start_x = 0 if relative else self.pos_x
start_y = 0 if relative else self.pos_y
start_z = 0 if relative else self.pos_z
return np.sqrt((x - start_x)**2 + (y - start_y)**2 + (z - start_z)**2)
def _sanitize_velocity(self, velocity: float) -> float:
return min(velocity, self._max_velocity)
def _sanitize_duration(self, duration: float) -> float:
return max(duration, self._min_duration)
def _sanitize_yaw(self, yaw: float) -> float:
return yaw % (2 * self._max_yaw_rotations * math.pi)
def _sanitize_x(self, x: float, relative: bool) -> float:
target_x = (self.pos_x + x) if relative else x
sanitized_x = self._sanitize_number(target_x, self._arena.min_x,
self._arena.max_x)
return self._arena.transform_x(sanitized_x)
def _sanitize_y(self, y: float, relative: bool) -> float:
target_y = (self.pos_y + y) if relative else y
sanitized_y = self._sanitize_number(target_y, self._arena.min_y,
self._arena.max_y)
return self._arena.transform_y(sanitized_y)
def _sanitize_z(self, z: float, relative: bool) -> float:
return self._sanitize_number(z, self._arena.min_z, self._arena.max_z)
def _sanitize_number(self, value: float, min_value: float,
max_value: float) -> float:
return min(max(value, min_value), max_value)
class Drone:
thruster = 0
yaw = 0
pitch = 0
roll = 0
connected = False
thrust_flag = False
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):
""" 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()
print "connected to %s" % link_uri
print("before Logconfig")
Drone.connected = 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
def _motor_control(self):
print "Motor control started!!"
base_thrust_value = 35000
thrust_step = 8000
thrust = base_thrust_value
pitch_rate = 10
roll_rate = 10
yaw_rate = 0
# win = pg.GraphicsWindow()
# win.setWindowTitle('pyqtgraph example: Scrolling Plots')
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=100)
self._lg_stab.add_variable("stabilizer.thrust", "float")
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."
while True:
self._cf.param.set_value("flightmode.althold", "True")
self._cf.commander.send_setpoint(Drone.roll, Drone.pitch,
Drone.yaw, thrust)
# thrust = base_thrust_value
# if self.thrust_flag:
# self._cf.param.set_value("flightmode.althold", "False")
"""
if thrust < 65000 and Drone.thruster == 1:
thrust += thrust_step * Drone.thruster
Drone.thruster = 0
elif thrust + thrust_step * Drone.thruster > 35000 and Drone.thruster == -1:
thrust += thrust_step * Drone.thruster
Drone.thruster = 0
"""
if 35000 < thrust + thrust_step * Drone.thruster <= 65000:
thrust += thrust_step * Drone.thruster
Drone.thruster = 0
Drone.thruster = 0
self.thrust_flag = False
# else:
# self._cf.param.set_value("flightmode.althold", "True")
Drone.pitch = Drone.roll = 0
# time.sleep(0.1)
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()
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 "Log: [%d][%s]: %s" % (timestamp, logconf.name, data)
print(data['stabilizer.roll'])
self.roll = -data['stabilizer.roll']
self.pitch = -data['stabilizer.pitch']
self.yaw = -data['stabilizer.yaw']
f_thrust = open('data_log_thrust.txt', 'a')
f_roll = open('data_log_roll.txt', 'a')
f_pitch = open('data_log_pitch.txt', 'a')
f_yaw = open('data_log_yaw.txt', 'a')
f_data = open('data_log.txt', 'a')
f_thrust.write(str(data['stabilizer.thrust']) + ",")
f_roll.write(str(data['stabilizer.roll']) + ",")
f_pitch.write(str(data['stabilizer.pitch']) + ",")
f_yaw.write(str(data['stabilizer.yaw']) + ",")
f_data.write(
str(data['stabilizer.thrust']) + "," +
str(data['stabilizer.roll']) + "," +
str(data['stabilizer.pitch']) + "," + str(data['stabilizer.yaw']) +
"\n")
f_roll.close()
f_pitch.close()
f_yaw.close()
f_thrust.close()
f_data.close()
class CrazyflieControl:
"""
Class that connects to a Crazyflie and controls it until disconnection.
"""
#------------------------------------------------------------------------------
# Initialization
#------------------------------------------------------------------------------
def __init__(self, link_uri):
"""
Initializes the control class and executes all needed functions.
"""
# Connect Crazyflie
self.Crazyflie = Crazyflie()
self.Connected = False
self.Connect(link_uri)
while not self.Connected:
wait(0.1)
pass
# Start Program
self.t0 = 0#getTime()
self.Running = True
# Initialize
self.SetInitialState()
self.InitializeReferenceCS()
if Plotting:
self.InitializePlotting()
if GUI:
self.InitializeGUI()
if Animation:
self.InitializeAnimation()
# Run Main Loops
Thread(target=self.MainLoop).start()
if GUI:
Thread(target=self.GUILoop).start()
if Animation:
Thread(target=self.AnimationLoop).start()
def SetInitialState(self):
'''
Sets all initial control variables to be used later on.
'''
# Intial Values
self.thrust_mult = 1
self.thrust_step = 100#250#500
# Lift-off at approx. 45000
self.thrust = 25e3#35e3#25e3#35e3#40e3#36e3#22e3#-#50e3#48e3#45e3#35e3#30e3#20e3
self.thrustmin = 23e3#30e3#23e3#30e3#38e3#30e3#20e3#-#30e3
self.thrustmax = 26e3#38e3#26e3#38e3#42e3#38e3#30e3#-#55e3#50e3#45e3#40e3#38e3#35e3#3e30#45e3#25e3
self.pitch = 0
self.roll = 0
self.yawrate = 0
# self.thrust_mult = thrust_mult
# self.thrust_step = thrust_step
# self.thrust = thrust
# self.thrustmin = thrustmin
# self.thrustmax = thrustmax
# self.pitch = pitch
# self.roll = roll
# self.yawrate = yawrate
self.roll_Real = 0.
self.pitch_Real = 0.
self.yaw_Real = 0.
self.att = np.zeros(3)
self.pos = np.zeros(3)
self.vel = np.zeros(3)
self.a = np.zeros(3)
self.velRel = np.zeros(3)
self.aRel = np.zeros(3)
self.aRaw = np.zeros(3)
self.a_DataTimer = 0
self.baro = 0.
self.baro_0 = 0.
self.battery = 0.
self.motors = np.zeros(4)
self.gyro = np.zeros(3)
self.mag = np.zeros(3)
PrintInfo("Initial Values Set")
def InitializeReferenceCS(self):
'''
Initializes the Coordinate Systems of the Crazyflie
'''
self.refCS = np.array([\
[0,1,0],\
[-1,0,0],\
[0,0,1]\
])
self.CS0 = CoordinateSystem(refCS=self.refCS,RotationSigns=[1,1,-1],name="Default Coordinate System")
self.CS = CoordinateSystem(refCS=self.refCS,RotationSigns=[1,1,-1],name="Crazyflie Coordinate System")
PrintInfo("Reference Systems initialized")
def InitializeGUI(self):
'''
Initializes the GUI for the state representation and user interaction
'''
self.InitStateWindow()
self.CrazyflieDataText=""
PrintInfo("GUI initialized")
def InitializeAnimation(self):
'''
Initializes the Animation Window and the 3D Model of the Crazyflie
'''
self.Model3D = None
self.Window = Window()
self.Create_3DModel()
PrintInfo("Animation initialized")
#------------------------------------------------------------------------------
# Crazyflie Connection Status Callbacks
#------------------------------------------------------------------------------
def Connect(self,link_uri):
self.Crazyflie.connected.add_callback(self.connected)
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)
self.Crazyflie.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.MainLoop).start()
self.Connected = True
PrintInfo("Connection to %s is established" % link_uri)
def connection_failed(self, link_uri, msg):
"""
Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)
"""
PrintError("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)
"""
PrintError("Connection to %s lost: %s" % (link_uri, msg))
def disconnected(self, link_uri):
"""
Callback when the Crazyflie is disconnected (called in all cases)
"""
PrintError("Disconnected from %s" % link_uri)
#------------------------------------------------------------------------------
# Main Loop
#------------------------------------------------------------------------------
def MainLoop(self):
self.run = 0
self.SetCalibratedState()
self.InitializeLogging()
self.InitializeGetParameters()
self.ControlLoop()
self.Kill()
def Kill(self):
print ">>>Killing Drone<<<"
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
self.Running = False
wait(0.1)
self.Crazyflie.close_link()
def KillCheck(self):
if abs(self.att[0])>=70 or abs(self.att[1])>=70:
print ">Angle Kill<"
self.Running=False
if getKey("Escape")!=0:
print ">Escape Kill<"
self.Running=False
# if self.thrust <= self.thrustmin:
# print ">ThrustMin Kill<"
# self.Running=False
#------------------------------------------------------------------------------
# Personal Control
#------------------------------------------------------------------------------
def ControlLoop(self):
while self.Running:
self.run+=1
# print "Run #"+str(self.run)
self.GetState()
#self.PrintState()
self.GetInfoText()
self.SendState()
self.EvaluateData()
self.KillCheck()
def GUILoop(self):
while self.Running:
if True:
# try:
self.StateWindow.fill([0]*3)
lines = self.CrazyflieDataText.split("\n")
for i in range(len(lines)):
line = lines[i]
render_text(self.StateWindow,line,[300,100+i*30],self.Font)
pg.display.flip()
pg.event.wait()
# except:
# PrintSpecial("Display Failed!","Red")
else:
self.PrintData()
def AnimationLoop(self):
if self.Model3D!=None:
self.Model3D.Update()
def SetCalibratedState(self):
if Calibrate:
# Positive: Front || Negative: Back
self.pitch = 4#5#-#40#38#35#32#28#-28
# Positive: Right || Negative: Left
self.roll = 0#-#-22#-20#-18#-15#-10#-2
def GetState(self):
# Climb & Decent
if self.thrust >= self.thrustmax:
self.thrust_mult = 0#-1
self.thrust += self.thrust_step * self.thrust_mult
self.thrust=0
if Stabilize:
# Stabilize
if self.roll_Real<self.roll:
self.roll+=1.
elif self.roll_Real>self.roll:
self.roll-=1.
if self.pitch_Real<self.pitch:
self.roll+=1.
elif self.pitch_Real>self.pitch:
self.pitch-=1.
def SendState(self):
self.Crazyflie.commander.send_setpoint(self.roll,self.pitch,self.yawrate,self.thrust)
wait(0.1)
def PrintState(self):
print "-"*30
print "Set Data:"
print PH+"Thrust: "+str(round(self.thrust,2))
print PH+"Roll: "+str(round(self.roll,2))
print PH+"Pitch: "+str(round(self.pitch,2))
print PH+"Yaw: "+str(round(self.yawrate,2))
#------------------------------------------------------------------------------
# Logging
#------------------------------------------------------------------------------
def EvaluateData(self):
self.att = np.array([self.roll_Real,-self.pitch_Real,-self.yaw_Real])
# Update CS
self.CS.setCS(self.att)
if Plotting:
if self.t0==0:
self.t0=getTime()
# print "Timestamp: ",timestamp
t = getTime()-self.t0
self.Stabilizer_Plot.UpdateGraph(graphname="Roll" ,pos=[t,np.degrees(self.roll_Real)])
self.Stabilizer_Plot.UpdateGraph(graphname="Pitch",pos=[t,np.degrees(self.pitch_Real)])
self.Stabilizer_Plot.UpdateGraph(graphname="Yaw" ,pos=[t,np.degrees(self.yaw_Real)])
self.Velocity_Plot.UpdateGraph(graphname="X" ,pos=[t,self.vel[0]])
self.Velocity_Plot.UpdateGraph(graphname="Y" ,pos=[t,self.vel[1]])
self.Velocity_Plot.UpdateGraph(graphname="Z" ,pos=[t,self.vel[2]])
#print self.aRaw[2],VectorAbs(self.aRaw)#self.a
self.Acceleration_Plot.UpdateGraph(graphname="X_Earth",pos=[t,self.a[0]])
self.Acceleration_Plot.UpdateGraph(graphname="Y_Earth",pos=[t,self.a[1]])
self.Acceleration_Plot.UpdateGraph(graphname="Z_Earth",pos=[t,self.a[2]])
self.Acceleration_Plot.UpdateGraph(graphname="X_Raw",pos=[t,self.aRaw[0]])
self.Acceleration_Plot.UpdateGraph(graphname="Y_Raw",pos=[t,self.aRaw[1]])
self.Acceleration_Plot.UpdateGraph(graphname="Z_Raw",pos=[t,self.aRaw[2]])
def InitializePlotting(self):
self.Stabilizer_Plot = Debug_Display("Attitude",u"°",x=0,y=0,w=SysInfo.Resolution[0]/2,h=SysInfo.Resolution[1]/3,IndicationInterval=5,IndicationRange=20)
self.Stabilizer_Plot.addGraph(name="Roll" ,color=Colors["Red"] ,label=False)
self.Stabilizer_Plot.addGraph(name="Pitch",color=Colors["Green"],label=False)
self.Stabilizer_Plot.addGraph(name="Yaw" ,color=Colors["Blue"] ,label=False)
self.Velocity_Plot = Debug_Display("Velocity",u"m/s",x=0,y=SysInfo.Resolution[1]/3,w=SysInfo.Resolution[0]/2,h=SysInfo.Resolution[1]/3,IndicationInterval=2,IndicationRange=8)
self.Velocity_Plot.addGraph(name="X",color=Colors["Red"] ,label=False)
self.Velocity_Plot.addGraph(name="Y",color=Colors["Green"],label=False)
self.Velocity_Plot.addGraph(name="Z",color=Colors["Blue"] ,label=False)
self.Acceleration_Plot = Debug_Display("Acceleration",u"m/s²",x=0,y=SysInfo.Resolution[1]*2/3,w=SysInfo.Resolution[0]/2,h=SysInfo.Resolution[1]/3,IndicationInterval=2,IndicationRange=8)
self.Acceleration_Plot.addGraph(name="X_Earth",color=Colors["Red"] ,label=False)
self.Acceleration_Plot.addGraph(name="Y_Earth",color=Colors["Green"],label=False)
self.Acceleration_Plot.addGraph(name="Z_Earth",color=Colors["Blue"] ,label=False)
self.Acceleration_Plot.addGraph(name="X_Raw",color=Colors["Red"] *0.5,label=False)
self.Acceleration_Plot.addGraph(name="Y_Raw",color=Colors["Green"]*0.5,label=False)
self.Acceleration_Plot.addGraph(name="Z_Raw",color=Colors["Blue"] *0.5,label=False)
def InitializeLogging(self):
# Attitude (if period_in_ms = <10 -> Invalid -> other Logs start working again?! O.o)
self.StabilizerLog = LogConfig(name="Stabilizer", period_in_ms=10)
self.StabilizerLog.add_variable("stabilizer.roll", "float")
self.StabilizerLog.add_variable("stabilizer.pitch", "float")
self.StabilizerLog.add_variable("stabilizer.yaw", "float")
self.Crazyflie.log.add_config(self.StabilizerLog)
# Battery
self.BatteryLog = LogConfig(name="Battery", period_in_ms=200)
self.BatteryLog.add_variable("pm.vbat", "float")
self.Crazyflie.log.add_config(self.BatteryLog)
# Barometer
self.BarometerLog = LogConfig(name="Barometer", period_in_ms=10)
self.BarometerLog.add_variable("baro.aslLong", "float")
self.Crazyflie.log.add_config(self.BarometerLog)
# Accelerometer (if period_in_ms = <10 -> Invalid -> other Logs start working again?! O.o)
self.AccelerometerLog = LogConfig(name="Accelerometer", period_in_ms=10)
self.AccelerometerLog.add_variable("acc.x", "float")
self.AccelerometerLog.add_variable("acc.y", "float")
self.AccelerometerLog.add_variable("acc.z", "float")
self.Crazyflie.log.add_config(self.AccelerometerLog)
# Motors
self.MotorsLog = LogConfig(name="Motors", period_in_ms=50)
self.MotorsLog.add_variable("motor.m1", "int32_t")
self.MotorsLog.add_variable("motor.m2", "int32_t")
self.MotorsLog.add_variable("motor.m3", "int32_t")
self.MotorsLog.add_variable("motor.m4", "int32_t")
self.Crazyflie.log.add_config(self.MotorsLog)
# Gyro
self.GyroLog = LogConfig(name="Gyro", period_in_ms=50)
self.GyroLog.add_variable("gyro.x", "float")
self.GyroLog.add_variable("gyro.y", "float")
self.GyroLog.add_variable("gyro.z", "float")
self.Crazyflie.log.add_config(self.GyroLog)
# Magnetometer
self.MagnetometerLog = LogConfig(name="Magnetometer", period_in_ms=50)
self.MagnetometerLog.add_variable("mag.x", "float")#int16_t")
self.MagnetometerLog.add_variable("mag.y", "float")#int16_t")
self.MagnetometerLog.add_variable("mag.z", "float")#int16_t")
self.Crazyflie.log.add_config(self.MagnetometerLog)
#wait(10)
if self.StabilizerLog.valid:
# This callback will receive the data
self.StabilizerLog.data_received_cb.add_callback(self.Stabilizer_log_data)
# This callback will be called on errors
self.StabilizerLog.error_cb.add_callback(self.log_error)
# Start the logging
self.StabilizerLog.start()
else:
print("Could not add logconfig '"+str(self.StabilizerLog.name)+"' since some variables are not in TOC")
wait(2)
if self.BatteryLog.valid:
# This callback will receive the data
self.BatteryLog.data_received_cb.add_callback(self.Battery_log_data)
# This callback will be called on errors
self.BatteryLog.error_cb.add_callback(self.log_error)
# Start the logging
self.BatteryLog.start()
else:
print("Could not add logconfig '"+str(self.BatteryLog.name)+"' since some variables are not in TOC")
wait(2)
if self.BarometerLog.valid:
# This callback will receive the data
self.BarometerLog.data_received_cb.add_callback(self.Barometer_log_data)
# This callback will be called on errors
self.BarometerLog.error_cb.add_callback(self.log_error)
# Start the logging
self.BarometerLog.start()
else:
print("Could not add logconfig '"+str(self.BarometerLog.name)+"' since some variables are not in TOC")
wait(2)
if self.AccelerometerLog.valid:
# This callback will receive the data
self.AccelerometerLog.data_received_cb.add_callback(self.Accelerometer_log_data)
# This callback will be called on errors
self.AccelerometerLog.error_cb.add_callback(self.log_error)
# Start the logging
self.AccelerometerLog.start()
else:
print("Could not add logconfig '"+str(self.AccelerometerLog.name)+"' since some variables are not in TOC")
wait(2)
if self.MotorsLog.valid:
# This callback will receive the data
self.MotorsLog.data_received_cb.add_callback(self.Motors_log_data)
# This callback will be called on errors
self.MotorsLog.error_cb.add_callback(self.log_error)
# Start the logging
self.MotorsLog.start()
else:
print("Could not add logconfig '"+str(self.MotorsLog.name)+"' since some variables are not in TOC")
wait(2)
if self.GyroLog.valid:
# This callback will receive the data
self.GyroLog.data_received_cb.add_callback(self.Gyro_log_data)
# This callback will be called on errors
self.GyroLog.error_cb.add_callback(self.log_error)
# Start the logging
self.GyroLog.start()
else:
print("Could not add logconfig '"+str(self.GyroLog.name)+"' since some variables are not in TOC")
wait(2)
if self.MagnetometerLog.valid:
# This callback will receive the data
self.MagnetometerLog.data_received_cb.add_callback(self.Magnetometer_log_data)
# This callback will be called on errors
self.MagnetometerLog.error_cb.add_callback(self.log_error)
# Start the logging
self.MagnetometerLog.start()
else:
print("Could not add logconfig '"+str(self.MagnetometerLog.name)+"' since some variables are not in TOC")
wait(2)
def Stabilizer_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
#print "[%d][%s]: %s" % (timestamp, logconf.name, data)
self.roll_Real = np.radians(data['stabilizer.roll'])
self.pitch_Real = np.radians(data['stabilizer.pitch'])
self.yaw_Real = np.radians(data['stabilizer.yaw'])
def Battery_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
self.battery = data['pm.vbat']
def Barometer_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
if self.baro_0==0:
self.baro_0 = copy(data['baro.aslLong'])
self.baro = data['baro.aslLong']-self.baro_0
def Accelerometer_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
if self.a_DataTimer==0:
self.a_DataTimer = getTime()
dt = getTime()-self.a_DataTimer
self.a_DataTimer = getTime()
self.aRaw = np.array([\
data['acc.x'],\
data['acc.y'],\
data['acc.z']\
]).astype(float)*10#/1000
# self.EvaluateAccelerometerData()
# def EvaluateAccelerometerData(self):
self.aRel = copy(self.aRaw)
#print self.aRel
Gravity = 9.81
relative_Gravity = [0,0,-Gravity]
relative_Gravity = EulerRotation(relative_Gravity,[1,0,0],self.roll_Real)
relative_Gravity = EulerRotation(relative_Gravity,[0,1,0],self.pitch_Real)
self.aRel+=relative_Gravity
# Body_to_Earth = rotation_matrix([1,0,0],self.att[0],matrix=True)*rotation_matrix([0,1,0],self.att[1],matrix=True)*rotation_matrix([0,0,1],self.att[2],matrix=True)
self.a=self.aRel#self.a = np.dot(Body_to_Earth,self.aRel)
self.velRel += self.aRel*dt
self.pos += self.velRel*dt
#print self.pos
def Motors_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
self.motors = np.array([\
data["motor.m1"],\
data["motor.m2"],\
data["motor.m3"],\
data["motor.m4"]\
])
def Gyro_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
self.gyro = np.array([\
data['gyro.x'],\
data['gyro.y'],\
data['gyro.z']\
])
def Magnetometer_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives"""
self.mag = [\
data['mag.x'],\
data['mag.y'],\
data['mag.z']\
]
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 PrintData(self):
print self.CrazyflieDataText
def GetInfoText(self):
self.CrazyflieDataText = ""
self.CrazyflieDataText += "-"*30+"\n"
self.CrazyflieDataText += "Received Data:"+"\n"
self.CrazyflieDataText += PH+"Roll: "+str(round(np.degrees(self.att[0]),3))+u" [°]"+"\n"#roll_Real ),3))+u" [°]"+"\n"
self.CrazyflieDataText += PH+"Pitch: "+str(round(np.degrees(self.att[1]),3))+u" [°]"+"\n"#pitch_Real),3))+u" [°]"+"\n"
self.CrazyflieDataText += PH+"Yaw: "+str(round(np.degrees(self.att[2]),3))+u" [°]"+"\n"#yaw_Real ),3))+u" [°]"+"\n"
self.CrazyflieDataText += "\n"
self.CrazyflieDataText += PH+"Accelerometer: ["+str(round(self.a [0],3))+","+str(round(self.a [1],3))+","+str(round(self.a [2],3))+"] ("+str(round(VectorAbs(self.a ),3))+")"+u" [m/s²]"+"\n"
self.CrazyflieDataText += PH+"Velocity: ["+str(round(self.vel[0],3))+","+str(round(self.vel[1],3))+","+str(round(self.vel[2],3))+"] ("+str(round(VectorAbs(self.vel),3))+")"+u" [m/s]"+"\n"
self.CrazyflieDataText += PH+"Position: ["+str(round(self.pos[0],3))+","+str(round(self.pos[1],3))+","+str(round(self.pos[2],3))+"] ("+str(round(VectorAbs(self.pos),3))+")"+u" [m]"+"\n"
self.CrazyflieDataText += "\n"
self.CrazyflieDataText += PH+"Barometer: "+str(round(self.baro ,3))+u" [m]" +" || Barometer_0: "+str(round(self.baro_0,3))+u" [m]" +"\n"+"\n"
self.CrazyflieDataText += PH+"Battery: "+str(round(self.battery*10**3,3))+u" [mV]"+"\n"
self.CrazyflieDataText += PH+"Motors: "+\
str(round(self.motors[0],3))+","+\
str(round(self.motors[1],3))+","+\
str(round(self.motors[2],3))+","+\
str(round(self.motors[3],3))+\
u" [?]" +"\n"
self.CrazyflieDataText += PH+"Gyro: "+str(round(self.gyro[0],3))+","+str(round(self.gyro[1],3))+","+str(round(self.gyro[2],3))+u" [?]" +"\n"
self.CrazyflieDataText += PH+"Magneto: "+str(round(self.mag[0] ,3))+","+str(round(self.mag[1] ,3))+","+str(round(self.mag[2] ,3))+u" [?]"+"\n"
# print "-"*30
# print "Received Data:"
# print PH+"Roll: "+str(round(self.roll_Real ,3))+u" [°]"
# print PH+"Pitch: "+str(round(self.pitch_Real ,3))+u" [°]"
# print PH+"Yaw: "+str(round(self.yaw_Real ,3))+u" [°]"
# # print
# # print PH+"Accelerometer: "+str(np.round(self.a,3))+u" [m/s²]"
# # print
# # print PH+"Baro: "+str(round(self.baro ,3))+u" [m]"
# # print PH+"Battery: "+str(round(self.battery*10**3,3))+u" [mV]"
# # print PH+"Motors: "+str(np.round(self.motors ,3))+u" [?]"
# # print PH+"Gyro: "+str(np.round(self.gyro ,3))+u" [?]"
# # print PH+"Magneto: s"+str(np.round(self.mag ,3))+u" [?]"
#------------------------------------------------------------------------------
# Get Parameters
#------------------------------------------------------------------------------
def InitializeGetParameters(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
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
self.param_check_list = []
self.param_groups = []
random.seed()
# Print the param TOC
self.CrazyflieParameters = dict()
p_toc = self.Crazyflie.param.toc.toc
for group in sorted(p_toc.keys()):
Group = "{}".format(group)
#print Group # <-------------------------------------------------------------------------------
self.CrazyflieParameters[Group] = dict()
for param in sorted(p_toc[group].keys()):
Parameter = "{}".format(param)
#print "\t"+Parameter # <-------------------------------------------------------------------------------
self.CrazyflieParameters[Group][Parameter] = 1
self.param_check_list.append("{0}.{1}".format(group, param))
self.param_groups.append("{}".format(group))
# For every group, register the callback
self.Crazyflie.param.add_update_callback(group=group, name=None,
cb=self.param_callback)
#self.PrintParameters()
# # You can also register a callback for a specific group.name combo
# self.Crazyflie.param.add_update_callback(group="cpu", name="flash",
# cb=self.cpu_flash_callback)
print
print "Reading back all parameter values"
# Request update for all the parameters using the full name
# group.name
for p in self.param_check_list:
self.Crazyflie.param.request_param_update(p)
# # Set LED-Ring Test
# wait(20)
# Color = [0,0,40]
# print "LED Ring effect changed to Color :"+str(Color)
# self.SetLED_RGB(Color)
# ringeffect = 1
# print "LED Ring effect changed to Mode #"+str(ringeffect)
# #self.Crazyflie.param.set_value("ring.effect","{:.2f}".format(ringeffect))
# SetParameter("ring","effect","1")
def SetParameter(self,Group,Parameter,Value):
# print "Set Parameter:",Group+"."+Parameter,"{:.2f}".format(Value)
# self.Crazyflie.param.set_value(Group+"."+Parameter,"{:.2f}".format(Value))
print "Set Parameter:",Group+"."+Parameter,"{:2}".format(Value)
self.Crazyflie.param.set_value(Group+"."+Parameter,"{:2}".format(Value))
def SetLED_RGB(self,Color):
self.SetParameter("ring","effect",str(7))
self.SetParameter("ring","solidRed" ,str(Color[0]))
self.SetParameter("ring","solidGreen",str(Color[1]))
self.SetParameter("ring","solidBlue" ,str(Color[2]))
print "LED color changed to: "+str(Color)
def PrintParameters(self):
print ">>>Crazyflie Parameters<<<"
for Group in self.CrazyflieParameters:
print Group
for Parameter in self.CrazyflieParamet,ers[Group]:
print PH+"- "+StringWithBlankspace(Parameter,20)+": "+str(self.CrazyflieParameters[Group][Parameter])
def cpu_flash_callback(self, name, value):
"""Specific callback for the cpu.flash parameter"""
print "The connected Crazyflie has {}kb of flash".format(value)
def param_callback(self, name, value):
"""Generic callback registered for all the groups"""
print "{0}: {1}".format(name, value) # <-------------------------------------------------------------------------------
# Group,Parameter=name.split(".")
# print ">",Group,Name,Value
# self.CrazyflieParameters[Group][Parameter] = value
Categories = name.split(".")
Group,Parameter = Categories[0],Categories[1]
#print ">",Group,Parameter,value#,Name,Value
self.CrazyflieParameters[Group][Parameter] = value
self.Crazyflie.param.add_update_callback(group=Group,name=Paramater,cb=self.parameter_update_callback)
# Remove each parameter from the list and close the link when
# all are fetched
self.param_check_list.remove(name)
if len(self.param_check_list) == 0:
print ">>>Have fetched all parameter values."
# First remove all the group callbacks
for g in self.param_groups:
self.Crazyflie.param.remove_update_callback(group=g,cb=self.param_callback)
# # Create a new random value [0.00,1.00] for pid_attitude.pitch_kd
# # and set it
# pkd = random.random()
# print
# print "Write: pid_attitude.pitch_kd={:.2f}".format(pkd)
# self.Crazyflie.param.add_update_callback(group="pid_attitude",
# name="pitch_kd",
# cb=self.a_pitch_kd_callback)
# # When setting a value the parameter is automatically read back
# # and the registered callbacks will get the updated value
# self.Crazyflie.param.set_value("pid_attitude.pitch_kd",
# "{:.2f}".format(pkd))
self._cf.param.add_update_callback(group="ring",name="effect",cb=self._a_ring_effect_callback)
self._cf.param.set_value("ring.effect","1")
self.PrintParameters()
# Set LED-Ring Test
Color = [0,0,40]
print "LED Ring effect changed to Color :"+str(Color)
self.SetLED_RGB(Color)
def _a_ring_effect_callback(self, name, value):
"""Callback for ring_effect"""
print "Readback (LED_effect): {0}={1}".format(name, value)
print
def parameter_update_callback(self,name,value):
print "Readback: {0}={1}".format(name, value)
#------------------------------------------------------------------------------
# 3D-Model
#------------------------------------------------------------------------------
def Create_3DModel(self):
self.Model3D = Crazyflie3D(self)
def InitStateWindow(self):
init_pygame()
pos = [0,0]#[100,100]#[0,0]
resolution = [SysInfo.Resolution[0]/4,SysInfo.Resolution[1]]
#PrintSpecial("Opening State Window","Red")
self.StateWindow = scr_init(pos=pos,reso=resolution,fullscreen=False,frameless=False,mouse=True,caption="Crazyflie Status")#,icon="Instrument Icon.png")
#PrintSpecial("State Window Opened","Red")
font='LCD.TTF'
self.Font = pg.font.SysFont(font,30)#'fonts/'+
class Main:
# Initial values, you can use these to set trim etc.
roll = 0.0
pitch = 0.0
yaw = 0
thrust = 15000
def __init__(self):
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers()
dev = sys.argv[1]
print 'device tag: %s' % dev
# You may need to update this value if your Crazyradio uses a different frequency.
self.crazyflie.open_link("radio://0/" + dev + "/2M")
self.crazyflie.connected.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
# Keep the commands alive so the firmware kill-switch doesn't kick in.
Thread(target=self.pulse_command).start()
print "Now accepting input\n>"
while 1:
val = getch.getch()
if val == 'w':
self.pitchUp()
elif val == 's':
self.pitchDown()
elif val == 'a':
self.rollDown()
elif val == 'd':
self.rollUp()
elif val == 'q':
self.yawUp()
elif val == 'e':
self.yawDown()
elif val == 'o':
self.thrustDown()
elif val == 'p':
self.thrustUp()
elif val == 'x':
self.shutdown()
def pulse_command(self):
while True:
self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust)
time.sleep(0.5)
def pitchUp(self):
self.pitch = self.pitch + 1
print 'pitch up', self.pitch
def pitchDown(self):
self.pitch = self.pitch - 1
print 'pitch down', self.pitch
def rollUp(self):
self.roll = self.roll + 1
print 'roll up', self.roll
def rollDown(self):
self.roll = self.roll - 1
print 'roll down', self.roll
def yawUp(self):
self.yaw = self.yaw + 1
print 'yaw up', self.yaw
def yawDown(self):
self.yaw = self.yaw - 1
print 'yaw down', self.yaw
def thrustUp(self):
self.thrust = self.thrust + 1000
if(self.thrust > 60000):
self.thrust = 60000
print 'thrust up', self.thrust
def thrustDown(self):
self.thrust = self.thrust - 1000
if(self.thrust < 10000):
self.thrust = 10000
print 'thrust down', self.thrust
def shutdown(self):
self.thrust = 10000
self.yaw = 0
self.roll = 0
self.pitch = 0
print "Resetting controls and closing link"
self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust)
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 = None
self._disconnect_event = None
self._is_link_open = False
self._error_message = None
def open_link(self):
if (self.is_link_open()):
raise Exception('Link already open')
self._add_callbacks()
logger.debug('Connecting to %s' % self._link_uri)
self._connect_event = Event()
self.cf.open_link(self._link_uri)
self._connect_event.wait()
self._connect_event = None
if not self._is_link_open:
self._remove_callbacks()
raise Exception(self._error_message)
def __enter__(self):
self.open_link()
return self
def close_link(self):
if (self.is_link_open()):
self._disconnect_event = Event()
self.cf.close_link()
self._disconnect_event.wait()
self._disconnect_event = None
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."""
logger.debug('Connected to %s' % link_uri)
self._is_link_open = True
if self._connect_event:
self._connect_event.set()
def _connection_failed(self, link_uri, msg):
"""Callback when initial connection fails (i.e no Crazyflie
at the specified address)"""
logger.debug('Connection to %s failed: %s' % (link_uri, msg))
self._is_link_open = False
self._error_message = msg
if self._connect_event:
self._connect_event.set()
def _disconnected(self, link_uri):
self._remove_callbacks()
self._is_link_open = False
if self._disconnect_event:
self._disconnect_event.set()
def _add_callbacks(self):
self.cf.connected.add_callback(self._connected)
self.cf.connection_failed.add_callback(self._connection_failed)
self.cf.disconnected.add_callback(self._disconnected)
def _remove_callbacks(self):
def remove_callback(container, callback):
try:
container.remove_callback(callback)
except ValueError:
pass
remove_callback(self.cf.connected, self._connected)
remove_callback(self.cf.connection_failed, self._connection_failed)
remove_callback(self.cf.disconnected, self._disconnected)
class MainUI(QtWidgets.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)
# Restore window size if present in the config file
try:
size = Config().get("window_size")
self.resize(size[0], size[1])
except KeyError:
pass
######################################################
# 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 - applying stylesheet")
tcss = """
QProgressBar {
border: 1px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: """ + COLOR_BLUE + """;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite - skinny bar stylesheet not applied")
######################################################
self.cf = Crazyflie(ro_cache=None,
rw_cache=cfclient.config_path + "/cache")
cflib.crtp.init_drivers(enable_debug_driver=Config()
.get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
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)
# Hide the 'File' menu on OS X, since its only item, 'Exit', gets
# merged into the application menu.
if sys.platform == 'darwin':
self.menuFile.menuAction().setVisible(False)
# Connect UI signals
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self.address.setValue(0xE7E7E7E7E7)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
self._disable_input = False
self.joystickReader.input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_setpoint(*args))
self.joystickReader.assisted_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_velocity_world_setpoint(*args))
self.joystickReader.heighthold_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_zdistance_setpoint(*args))
self.joystickReader.hover_input_updated.add_callback(
self.cf.commander.send_hover_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(self._disconnected)
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(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
self.linkQualityBar.setTextVisible(False)
self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# 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
cfclient.ui.pluginhelper.mainUI = self
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)
# Load and connect tabs
self.tabsMenuItem = QMenu("Tabs", self.menuView, enabled=True)
self.menuView.addMenu(self.tabsMenuItem)
# self.tabsMenuItem.setMenu(QtWidgets.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in cfclient.ui.tabs.available:
tab = tabClass(self.tabs, cfclient.ui.pluginhelper)
item = QtWidgets.QAction(tab.getMenuName(), self, checkable=True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.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 is not 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))
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxesMenuItem = QMenu("Toolboxes", self.menuView,
enabled=True)
self.menuView.addMenu(self.toolboxesMenuItem)
self.toolboxes = []
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 = QtWidgets.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node,)
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node,)
self._all_role_menus += ({"muxmenu": node,
"rolemenu": sub_node},)
node.setData((m, mux_subnodes))
self._mapping_support = True
def disable_input(self, disable):
"""
Disable the gamepad input to be able to send setpoint from a tab
"""
self._disable_input = disable
def interfaceChanged(self, interface):
if interface == INTERFACE_PROMPT_TEXT:
self._selected_interface = None
else:
self._selected_interface = interface
self._update_ui_state()
def foundInterfaces(self, interfaces):
selected_interface = self._selected_interface
self.interfaceCombo.clear()
self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT)
formatted_interfaces = []
for i in interfaces:
if len(i[1]) > 0:
interface = "%s - %s" % (i[0], i[1])
else:
interface = i[0]
formatted_interfaces.append(interface)
self.interfaceCombo.addItems(formatted_interfaces)
if self._initial_scan:
self._initial_scan = False
try:
if len(Config().get("link_uri")) > 0:
formatted_interfaces.index(Config().get("link_uri"))
selected_interface = Config().get("link_uri")
except KeyError:
# The configuration for link_uri was not found
pass
except ValueError:
# The saved URI was not found while scanning
pass
if len(interfaces) == 1 and selected_interface is None:
selected_interface = interfaces[0][0]
newIndex = 0
if selected_interface is not None:
try:
newIndex = formatted_interfaces.index(selected_interface) + 1
except ValueError:
pass
self.interfaceCombo.setCurrentIndex(newIndex)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _update_ui_state(self):
if self.uiState == UIState.DISCONNECTED:
self.setWindowTitle("Not connected")
canConnect = self._selected_interface is not None
self.menuItemConnect.setText("Connect to Crazyflie")
self.menuItemConnect.setEnabled(canConnect)
self.connectButton.setText("Connect")
self.connectButton.setToolTip(
"Connect to the Crazyflie on the selected interface")
self.connectButton.setEnabled(canConnect)
self.scanButton.setText("Scan")
self.scanButton.setEnabled(True)
self.address.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)
self.interfaceCombo.setEnabled(True)
elif self.uiState == UIState.CONNECTED:
s = "Connected on %s" % self._selected_interface
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Disconnect")
self.connectButton.setToolTip("Disconnect from the Crazyflie")
self.scanButton.setEnabled(False)
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)
elif self.uiState == UIState.CONNECTING:
s = "Connecting to {} ...".format(self._selected_interface)
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Cancel")
self.connectButton.setToolTip("Cancel connecting to the Crazyflie")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.setEnabled(False)
elif self.uiState == UIState.SCANNING:
self.setWindowTitle("Scanning ...")
self.connectButton.setText("Connect")
self.menuItemConnect.setEnabled(False)
self.connectButton.setText("Connect")
self.connectButton.setEnabled(False)
self.scanButton.setText("Scanning...")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.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_battery(self, timestamp, data, logconf):
self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
color = COLOR_BLUE
# TODO firmware reports fully-charged state as 'Battery',
# rather than 'Charged'
if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]:
color = COLOR_GREEN
elif data["pm.state"] == BatteryStates.LOW_POWER:
color = COLOR_RED
self.batteryBar.setStyleSheet(progressbar_stylesheet(color))
def _connected(self):
self.uiState = UIState.CONNECTED
self._update_ui_state()
Config().set("link_uri", str(self._selected_interface))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
lg.add_variable("pm.state", "int8_t")
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))
mems = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)
if len(mems) > 0:
mems[0].write_data(self._led_write_done)
def _disconnected(self):
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _connection_initiated(self):
self.uiState = UIState.CONNECTING
self._update_ui_state()
def _led_write_done(self, mem, addr):
logger.info("LED write done callback")
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.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._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.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._connect()
def closeEvent(self, event):
self.hide()
self.cf.close_link()
Config().save_file()
def resizeEvent(self, event):
Config().set("window_size", [event.size().width(),
event.size().height()])
def _connect(self):
if self.uiState == UIState.CONNECTED:
self.cf.close_link()
elif self.uiState == UIState.CONNECTING:
self.cf.close_link()
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self.cf.open_link(self._selected_interface)
def _scan(self):
self.uiState = UIState.SCANNING
self._update_ui_state()
self.scanner.scanSignal.emit(self.address.value())
def _display_input_device_error(self, error):
self.cf.close_link()
QMessageBox.critical(self, "Input device error", error)
def _mux_selected(self, checked):
"""Called when a new mux is selected. The menu item contains a
reference to the raw mux object as well as to the associated device
sub-nodes"""
if not checked:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(False)
else:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(True)
self.joystickReader.set_mux(mux=mux)
# Go though the tree and select devices/mapping that was
# selected before it was disabled.
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
dev_node.toggled.emit(True)
self._update_input_device_footer()
def _get_dev_status(self, device):
msg = "{}".format(device.name)
if device.supports_mapping:
map_name = "No input mapping"
if device.input_map:
map_name = device.input_map_name
msg += " ({})".format(map_name)
return msg
def _update_input_device_footer(self):
"""Update the footer in the bottom of the UI with status for the
input device and its mapping"""
msg = ""
if len(self.joystickReader.available_devices()) > 0:
mux = self.joystickReader._selected_mux
msg = "Using {} mux with ".format(mux.name)
for key in list(mux._devs.keys())[:-1]:
if mux._devs[key]:
msg += "{}, ".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A, "
# Last item
key = list(mux._devs.keys())[-1]
if mux._devs[key]:
msg += "{}".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A"
else:
msg = "No input device found"
self._statusbar_label.setText(msg)
def _inputdevice_selected(self, checked):
"""Called when a new input device has been selected from the menu. The
data in the menu object is the associated map menu (directly under the
item in the menu) and the raw device"""
(map_menu, device, mux_menu) = self.sender().data()
if not checked:
if map_menu:
map_menu.setEnabled(False)
# Do not close the device, since we don't know exactly
# how many devices the mux can have open. When selecting a
# new mux the old one will take care of this.
else:
if map_menu:
map_menu.setEnabled(True)
(mux, sub_nodes) = mux_menu.data()
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
if device.id == dev_node.data()[1].id \
and dev_node is not self.sender():
dev_node.setChecked(False)
role_in_mux = str(self.sender().parent().title()).strip()
logger.info("Role of {} is {}".format(device.name,
role_in_mux))
Config().set("input_device", str(device.name))
self._mapping_support = self.joystickReader.start_input(
device.name,
role_in_mux)
self._update_input_device_footer()
def _inputconfig_selected(self, checked):
"""Called when a new configuration has been selected from the menu. The
data in the menu object is a referance to the device QAction in parent
menu. This contains a referance to the raw device."""
if not checked:
return
selected_mapping = str(self.sender().text())
device = self.sender().data().data()[1]
self.joystickReader.set_input_map(device.name, selected_mapping)
self._update_input_device_footer()
def device_discovery(self, devs):
"""Called when new devices have been added"""
for menu in self._all_role_menus:
role_menu = menu["rolemenu"]
mux_menu = menu["muxmenu"]
dev_group = QActionGroup(role_menu, exclusive=True)
for d in devs:
dev_node = QAction(d.name, role_menu, checkable=True,
enabled=True)
role_menu.addAction(dev_node)
dev_group.addAction(dev_node)
dev_node.toggled.connect(self._inputdevice_selected)
map_node = None
if d.supports_mapping:
map_node = QMenu(" Input map", role_menu, enabled=False)
map_group = QActionGroup(role_menu, exclusive=True)
# Connect device node to map node for easy
# enabling/disabling when selection changes and device
# to easily enable it
dev_node.setData((map_node, d))
for c in ConfigManager().get_list_of_configs():
node = QAction(c, map_node, checkable=True,
enabled=True)
node.toggled.connect(self._inputconfig_selected)
map_node.addAction(node)
# Connect all the map nodes back to the device
# action node where we can access the raw device
node.setData(dev_node)
map_group.addAction(node)
# If this device hasn't been found before, then
# select the default mapping for it.
if d not in self._available_devices:
last_map = Config().get("device_config_mapping")
if d.name in last_map and last_map[d.name] == c:
node.setChecked(True)
role_menu.addMenu(map_node)
dev_node.setData((map_node, d, mux_menu))
# Update the list of what devices we found
# to avoid selecting default mapping for all devices when
# a new one is inserted
self._available_devices = ()
for d in devs:
self._available_devices += (d,)
# Only enable MUX nodes if we have enough devies to cover
# the roles
for mux_node in self._all_mux_nodes:
(mux, sub_nodes) = mux_node.data()
if len(mux.supported_roles()) <= len(self._available_devices):
mux_node.setEnabled(True)
# TODO: Currently only supports selecting default mux
if self._all_mux_nodes[0].isEnabled():
self._all_mux_nodes[0].setChecked(True)
# If the previous length of the available devies was 0, then select
# the default on. If that's not available then select the first
# on in the list.
# TODO: This will only work for the "Normal" mux so this will be
# selected by default
if Config().get("input_device") in [d.name for d in devs]:
for dev_menu in self._all_role_menus[0]["rolemenu"].actions():
if dev_menu.text() == Config().get("input_device"):
dev_menu.setChecked(True)
else:
# Select the first device in the first mux (will always be "Normal"
# mux)
self._all_role_menus[0]["rolemenu"].actions()[0].setChecked(True)
logger.info("Select first device")
self._update_input_device_footer()
def _open_config_folder(self):
QDesktopServices.openUrl(
QUrl("file:///" +
QDir.toNativeSeparators(cfclient.config_path)))
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()
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)
np.array([
self.odometry.roll, self.odometry.pitch, self.odometry.yaw
]) * np.pi / 180)
f = np.dot(
np.dot(R,
np.transpose(-kz * e_r - kvz * e_v + mass * g * e_3)),
e_3)
roll_cmd = -k_theta * self.odometry.roll
pitch_cmd = -k_theta * self.odometry.pitch
yaw_cmd = -k_theta * self.odometry.yaw
a = 2.13e-11
b = 1.032e-6
c = 5.485e-4
if f > 0:
thrust = int(f / (0.057 * g) * 65535)
if thrust >= 35535:
thrust = 35534
else:
thrust = 0
print(e_r, e_v, f, thrust)
self.cf.commander.send_setpoint(roll_cmd, pitch_cmd, yaw_cmd,
thrust)
time.sleep(self.rate)
self.cf.high_level_commander.land(0.0, 1.0)
pid = PID_controller(cf)
pid.goto([0, 0, 0.3])
cf.close_link()
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 """
self._cf = Crazyflie(rw_cache='./cache')
# 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)
self.pubLS = rospy.Publisher('cf_tof_ranges', LaserScan, queue_size=10)
self.pubRup = rospy.Publisher('cf_tof_up', Range, queue_size=10)
rospy.init_node('talker', anonymous=True)
self.pubLS_msg = LaserScan()
self.pubLS_msg.angle_min = 0.0
self.pubLS_msg.angle_max = 3.14159 / 2 * 3
self.pubLS_msg.angle_increment = 3.14159 / 2
self.pubLS_msg.range_min = 0.0
self.pubLS_msg.range_max = 100.0
self.pubLS_msg.header.frame_id = 'cf_base'
self.range_msg_up = Range()
self.range_msg_up.header.frame_id = 'cf_base'
self.range_msg_up.min_range = 0.0
self.range_msg_up.max_range = 100.0
self.range_msg_up.field_of_view = 0.131 # 7.5 degrees
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
signal.signal(signal.SIGINT, self._signal_handler)
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='RANGES', period_in_ms=10)
self._lg_stab.add_variable('range.back', 'uint16_t')
self._lg_stab.add_variable('range.left', 'uint16_t')
self._lg_stab.add_variable('range.front', 'uint16_t')
self._lg_stab.add_variable('range.right', 'uint16_t')
self._lg_stab.add_variable('range.up', 'uint16_t')
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
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.')
# except KeyboardInterrupt:
# rethrow()
# 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))
#rangeStr = '[%d][%s]: %s' % (timestamp, logconf.name, data)
#self.pub.publish(rangeStr)
now = rospy.get_rostime()
self.pubLS_msg.header.stamp.secs = now.secs
self.pubLS_msg.header.stamp.nsecs = now.nsecs
self.pubLS_msg.ranges = [
data['range.front'] * 0.001, data['range.left'] * 0.001,
data['range.back'] * 0.001, data['range.right'] * 0.001
]
#self.pubLS.publish(self.pubLS_msg)
self.range_msg_up.range = data['range.up'] * 0.001
self.range_msg_up.header.stamp.secs = now.secs
self.range_msg_up.header.stamp.nsecs = now.nsecs
#self.pubRup.publish(self.range_msg_up)
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 _signal_handler(self, sig, frame):
print('You pressed Ctrl+C!')
self.is_connected = False
self._cf.close_link()
sys.exit(0)
class CrazyflieROS:
Disconnected = 0
Connecting = 1
Connected = 2
"""Wrapper between ROS and Crazyflie SDK"""
def __init__(self, link_uri, tf_prefix, roll_trim, pitch_trim):
self.link_uri = link_uri
self.tf_prefix = tf_prefix
self.roll_trim = roll_trim
self.pitch_trim = pitch_trim
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._cmdVel = Twist()
self._subCmdVel = rospy.Subscriber("cmd_vel", Twist,
self._cmdVelChanged)
self._pubImu = rospy.Publisher('imu', Imu, queue_size=10)
self._pubTemp = rospy.Publisher('temperature',
Temperature,
queue_size=10)
self._pubMag = rospy.Publisher('magnetic_field',
MagneticField,
queue_size=10)
self._pubPressure = rospy.Publisher('pressure', Float32, queue_size=10)
self._pubBattery = rospy.Publisher('battery', Float32, queue_size=10)
self._pubTest = rospy.Publisher('test', Float32, queue_size=10)
self._state = CrazyflieROS.Disconnected
Thread(target=self._update).start()
def _try_to_connect(self):
rospy.loginfo("Connecting to %s" % self.link_uri)
self._state = CrazyflieROS.Connecting
self._cf.open_link(self.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."""
rospy.loginfo("Connected to %s" % link_uri)
self._state = CrazyflieROS.Connected
self._lg_imu = LogConfig(name="IMU", period_in_ms=10)
self._lg_imu.add_variable("acc.x", "float")
self._lg_imu.add_variable("acc.y", "float")
self._lg_imu.add_variable("acc.z", "float")
self._lg_imu.add_variable("gyro.x", "float")
self._lg_imu.add_variable("gyro.y", "float")
self._lg_imu.add_variable("gyro.z", "float")
self._cf.log.add_config(self._lg_imu)
if self._lg_imu.valid:
# This callback will receive the data
self._lg_imu.data_received_cb.add_callback(self._log_data_imu)
# This callback will be called on errors
self._lg_imu.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_imu.start()
else:
rospy.logfatal(
"Could not add logconfig since some variables are not in TOC")
self._lg_log2 = LogConfig(name="LOG2", period_in_ms=100)
self._lg_log2.add_variable("mag.x", "float")
self._lg_log2.add_variable("mag.y", "float")
self._lg_log2.add_variable("mag.z", "float")
self._lg_log2.add_variable("baro.temp", "float")
self._lg_log2.add_variable("baro.pressure", "float")
self._lg_log2.add_variable("pm.vbat", "float")
self._lg_log2.add_variable("test.tval", "float")
self._cf.log.add_config(self._lg_log2)
if self._lg_log2.valid:
# This callback will receive the data
self._lg_log2.data_received_cb.add_callback(self._log_data_log2)
# This callback will be called on errors
self._lg_log2.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_log2.start()
else:
rospy.logfatal(
"Could not add logconfig since some variables are not in TOC")
p_toc = self._cf.param.toc.toc
for group in p_toc.keys():
self._cf.param.add_update_callback(group=group,
name=None,
cb=self._param_callback)
for name in p_toc[group].keys():
ros_param = "~{}/{}".format(group, name)
cf_param = "{}.{}".format(group, name)
if rospy.has_param(ros_param):
self._cf.param.set_value(cfparam,
rospy.get_param(ros_param))
else:
self._cf.param.request_param_update(cf_param)
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
rospy.logfatal("Connection to %s failed: %s" % (link_uri, msg))
self._state = CrazyflieROS.Disconnected
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
rospy.logfatal("Connection to %s lost: %s" % (link_uri, msg))
self._state = CrazyflieROS.Disconnected
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
rospy.logfatal("Disconnected from %s" % link_uri)
self._state = CrazyflieROS.Disconnected
def _log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
rospy.logfatal("Error when logging %s: %s" % (logconf.name, msg))
def _log_data_imu(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
msg = Imu()
# ToDo: it would be better to convert from timestamp to rospy time
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.tf_prefix + "/base_link"
msg.orientation_covariance[0] = -1 # orientation not supported
# measured in deg/s; need to convert to rad/s
msg.angular_velocity.x = math.radians(data["gyro.x"])
msg.angular_velocity.y = math.radians(data["gyro.y"])
msg.angular_velocity.z = math.radians(data["gyro.z"])
# measured in mG; need to convert to m/s^2
msg.linear_acceleration.x = data["acc.x"] * 9.81
msg.linear_acceleration.y = data["acc.y"] * 9.81
msg.linear_acceleration.z = data["acc.z"] * 9.81
self._pubImu.publish(msg)
#print "[%d][%s]: %s" % (timestamp, logconf.name, data)
def _log_data_log2(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
msg = Temperature()
# ToDo: it would be better to convert from timestamp to rospy time
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.tf_prefix + "/base_link"
# measured in degC
msg.temperature = data["baro.temp"]
self._pubTemp.publish(msg)
# ToDo: it would be better to convert from timestamp to rospy time
msg = MagneticField()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.tf_prefix + "/base_link"
# measured in Tesla
msg.magnetic_field.x = data["mag.x"]
msg.magnetic_field.y = data["mag.y"]
msg.magnetic_field.z = data["mag.z"]
self._pubMag.publish(msg)
msg = Float32()
# hPa (=mbar)
msg.data = data["baro.pressure"]
self._pubPressure.publish(msg)
# V
msg.data = data["pm.vbat"]
self._pubBattery.publish(msg)
# Test
msg.data = data["test.tval"]
self._pubTest.publish(msg)
def _param_callback(self, name, value):
ros_param = "~{}".format(name.replace(".", "/"))
rospy.set_param(ros_param, value)
def _send_setpoint(self):
roll = self._cmdVel.linear.y + self.roll_trim
pitch = self._cmdVel.linear.x + self.pitch_trim
yawrate = self._cmdVel.angular.z
thrust = max(10000, int(self._cmdVel.linear.z))
#print(roll, pitch, yawrate, thrust)
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
def _cmdVelChanged(self, data):
self._cmdVel = data
self._send_setpoint()
def _update(self):
while not rospy.is_shutdown():
if self._state == CrazyflieROS.Disconnected:
self._try_to_connect()
elif self._state == CrazyflieROS.Connected:
# Crazyflie will shut down if we don't send any command for 500ms
# Hence, make sure that we don't wait too long
# However, if there is no connection anymore, we try to get the flie down
if self._subCmdVel.get_num_connections() > 0:
self._send_setpoint()
else:
self._cmdVel = Twist()
self._send_setpoint()
rospy.sleep(0.2)
else:
rospy.sleep(0.5)
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
rospy.sleep(0.1)
self._cf.close_link()
class Sling:
"""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.thrust = 25000
self.pitch = 0
self.roll = 0
self.yawrate = 0
self.Run = False
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)
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Crazyflies and use the first one found
#print "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!
self.Run = True
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):
try:
thrust_mult = 1
thrust_step = 300
count = 0
#Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
while self.Run:
self._cf.commander.send_setpoint(self.roll, self.pitch,
self.yawrate, self.thrust)
time.sleep(0.2)
#print("sling:" + str(self.thrust))
"""if thrust >= 35000:
while count < 30:
time.sleep(0.05)
count += 1
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
if count >= 30:
thrust_mult = -1
thrust += thrust_mult*thrust_step"""
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
except:
print("Unexpected error:", sys.exc_info()[0])
raise
finally:
time.sleep(0.1)
self._cf.close_link()
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 = 20 # (Hz) Rate of each sending control input
self.statefb_rate = 50 # (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]]
# 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
def _run_controller(self):
""" Main control loop
# System parameters"""
m = 0.032 # [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.5 # for velocity
gp = 0.7 # for position
# Reference parameters
v0 = [0.0, 0.0, 0.0] # Reference velocity
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]
]
# Set the current reference to the current positional estimate
time.sleep(5)
# Hover at z = 0.3
self._cf.commander.send_hover_setpoint(0, 0, 0, 0.3)
time.sleep(2.0)
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]
# 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)
# 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.2:
v0 = [0.02, -0.02, 0.0]
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
#if time.time() > timeBeginCtrl + 10.0:
# v0 = [0.0, 0.0, -0.1]
# Landing
if time.time() > timeBeginCtrl + 11.0:
#self._cf.commander.send_setpoint(0, 0, 0, 0)
self._cf.commander.send_position_setpoint(x=x,
y=y,
z=0.0,
yaw=yaw)
time.sleep(0.5)
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
# Get roll, pitch, yaw
roll, pitch, yaw = self.attitude
x, y, z = self.position
vx, vy, vz = self.velocity
# Get eta
eta_px = x - (rot_mat[0][0] * fp[0] + rot_mat[0][1] * fp[1] +
rot_mat[0][2] * fp[2]) - coef_init * x_0
eta_py = y - (rot_mat[1][0] * fp[0] + rot_mat[1][1] * fp[1] +
rot_mat[1][2] * fp[2]) - coef_init * y_0
eta_pz = z - (rot_mat[2][0] * fp[0] + rot_mat[2][1] * fp[1] +
rot_mat[2][2] * fp[2]) - coef_init * z_0
eta_vx = vx - fv[0]
eta_vy = vy - fv[1]
eta_vz = vz - fv[2]
# Get u
u_x = -gp * delta * (eta_px - x_d) - gv * delta * (eta_vx - v0[0])
u_y = -gp * delta * (eta_py - y_d) - gv * delta * (eta_vy - v0[1])
u_z = -2 * gp * delta * (eta_pz - z_d) - 2 * gv * delta * (eta_vz -
v0[2])
u = [u_x, u_y, u_z]
self.datalog_Pos.write(
str(time.time()) + "," + str(self.position[0] - x_d) + "," +
str(self.position[1] - y_d) + "," +
str(self.position[2] - z_d) + "\n")
if self.isEnabled:
# Get thrust and attitude desired
thrust_d, roll_d, pitch_d = self.output2nd(u, yaw, m, g)
message = (
'ref:({}, {}, {})\n'.format(x_d, y_d, z_d) +
'pos:({}, {}, {})\n'.format(x, y, z) +
'att:({}, {}, {})\n'.format(roll, pitch, yaw) +
'control:({}, {}, {})\n'.format(roll_d, pitch_d, thrust_d))
else:
thrust_d, roll_d, pitch_d, yaw_d = (0.0, 0.0, 0.0, 0.0)
self._cf.close_link()
#print('Force disconnecting')
# Send set point
self._cf.commander.send_setpoint(roll_d, pitch_d, yaw_d, thrust_d)
#elf._cf.commander.send_setpoint(roll_d, pitch_d, yaw_d, 43000)
self.print_at_period(2.0, message)
self.loop_sleep(timeStart)
# End
def output2nd(self, u_out, yaw, m, g):
""" This calculates the thrust force and attitude desired """
# Calculate tau
tau1 = m * (cos(yaw) * u_out[0] + sin(yaw) * u_out[1])
tau2 = m * (-sin(yaw) * u_out[0] + cos(yaw) * u_out[1])
tau3 = m * (u_out[2] + g)
# Calculate thrust and attitude desired
thrust = sqrt(tau1 * tau1 + tau2 * tau2 + tau3 * tau3)
roll_d = asin(-tau2 / thrust)
pitch_d = atan2(tau1, tau3)
# thrust in 16bit and angle to degree
thrust_16bit_limit = self.thrust2cmd(thrust)
roll_d = roll_d * 180 / self.pi
pitch_d = pitch_d * 180 / self.pi
# Saturation of roll and pitch desired
roll_d_limit = self.saturation(roll_d, self.roll_limit)
pitch_d_limit = self.saturation(pitch_d, self.pitch_limit)
return [thrust_16bit_limit, roll_d_limit, pitch_d_limit]
def thrust2cmd(self, thrust):
""" This is able to transform thrust in Newton to command in integer 0-65000
We need to solve the second order polynominal to find
cmd_thrust in integer corresponding to thrust in Newton
thrust = 4 * (a * cmd_thrust ^ 2 + b * cmd_thrust + c)"""
a = 2.130295e-11
b = 1.032633e-6
c = 5.48456e-4
delta = b * b - 4 * a * (c - thrust / 4.0)
cmd_thrust = (-b + sqrt(delta)) / (2 * a)
cmd_thrust = int(round(cmd_thrust))
cmd_thrust = self.saturation(cmd_thrust, self.thrust_limit)
return cmd_thrust
def saturation(self, value, limits):
""" Saturate a given value within limit interval"""
if value < limits[0]:
value = limits[0]
#print("limit low")
elif value > limits[1]:
value = limits[1]
#print("limit up")
return value
def print_at_period(self, period, message):
""" Prints the message at a given period"""
if (time.time() - period) > self.timePrint:
self.timePrint = time.time()
print(message)
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")
# 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')
# Invoke logged states
self._cf.log.add_config(logPos)
self._cf.log.add_config(logVel)
self._cf.log.add_config(logAtt)
# 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()
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 _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 display(self):
plt.scatter(time.time() - self.timeplt, self.position[2])
plt.pause(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 crazy_command:
"""Example that connects to a Crazyflie and send command to the motors and
the disconnects"""
global commandsToGo
copter_index = 0
def __init__(self, link_uri, copter_index):
""" Initialize and run the example with the specified link_uri """
self.copter_index = copter_index
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.is_connected = True
print('Connecting to %s' % link_uri)
self._cf.commander.send_setpoint(0, 0, 0, 0)
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._send_commands_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))
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 _command_motors(self):
# Unlock startup thrust protection
roll = commandsToGo[0][0]
pitch = commandsToGo[0][1]
yawrate = commandsToGo[0][3]
thrust = commandsToGo[0][2]
# print(thrust)
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# 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()
def _send_commands(self, commands):
# roll = commandsToGo[0][0]
# pitch = commandsToGo[0][1]
# yawrate = commandsToGo[0][3]
# thrust = commandsToGo[0][2]
self._cf.commander.send_setpoint(commands[0], commands[1], commands[3],
commands[2])
# print(commands[2])
def _send_commands_thread(self):
# roll = commandsToGo[0][0]
# pitch = commandsToGo[0][1]
# yawrate = commandsToGo[0][3]
# thrust = commandsToGo[0][2]
while (self.is_connected):
self._cf.commander.send_setpoint(
commandsToGo[self.copter_index][0],
commandsToGo[self.copter_index][1],
commandsToGo[self.copter_index][3],
commandsToGo[self.copter_index][2])
time.sleep(0.005)
# time.sleep(0.01)
# print(commands[2])
def _close_it(self):
self._cf.close_link()
self.is_connected = False
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 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 _ramp_motors(self):
thrust_mult = 1
thrust_step = 500
thrust = 32000
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
print("Constant thrust")
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(1.2)
print("Turning on altitude hold")
self._cf.param.set_value("flightmode.althold", "True")
self._cf.commander.send_setpoint(0, 0, 0, 32767)
time.sleep(3)
print("Closing link")
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
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):
""" 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 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):
print("connected to %s" % link_uri)
self._roll_list = [0.0,0.0,0.0]
self._pitch_list = [0.0,0.0,0.0]
self._yaw_list = [0.0,0.0,0.0]
self.roll_i = 0
self.pitch_i = 0
self.roll = 0
self.pitch = 0
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")
try:
self._cf.log.add_config(self._lg_stab)
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
self._lg_stab.error_cb.add_callback(self._stab_log_error)
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.")
self._power_on = True
self._suspend = False
motor_thrd = Thread(target=self._ramp_motors)
cmd_thrd = Thread(target=self._controlcenter)
motor_thrd.start()
cmd_thrd.start()
def _controlcenter(self):
time.sleep(2)
while True:
cmd=raw_input("cmd->")
if cmd == "k" or cmd == "kill":
self._power_on=False
print("coptor-> stoped by the command \'kill\'\n")
if cmd == "s" or cmd == "suspend":
self._suspend = True
print("coptor-> suspendind\n")
if cmd == "l" or cmd == "land":
self._suspend = False
print("coptor-> landing\n")
def _ramp_motors(self):
try:
time.sleep(2)
inithrust = 32000
maxthrust = 38000
minthrust = 27000
duration = 5
f = open('./log','w')
self._ramp_takeoff(inithrust, maxthrust, f)
self._ramp_suspend(maxthrust, duration, f)
self._ramp_landing(maxthrust, minthrust, f)
print("mission complete\n")
sys.stdout.flush()
time.sleep(0.5)
except AttributeError as attr_error:
print("\t!! we confronted with an AttributeError: %s. link closed." % str(attr_error))
except TypeError as type_error:
print("\t!! we confronted with an TypeError: %s. link closed." % str(type_error))
finally:
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.5)
self._cf.close_link()
f.close()
def _ramp_takeoff(self, inithrust, maxthrust, logfile):
thrust = inithrust
thrust_step = 900
f = logfile
self._cf.commander.send_setpoint(0,0,0,0)
while thrust <= maxthrust and self._power_on:
(roll,pitch,yawrate) = self._pid(self._roll_list,self._pitch_list)
thrust += thrust_step
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
f.write('roll: \t\t'+str(self._roll_list[0])+'\t\t\t'+str(self._roll_list[1])+'\t\t\t'+str(self._roll_list[2])+'\n')
f.write('pitch: \t\t'+str(self._pitch_list[0])+'\t\t\t'+str(self._pitch_list[1])+'\t\t\t'+str(self._pitch_list[2])+'\n')
f.write('yaw: \t\t'+str(self._yaw_list[0])+'\t\t\t'+str(self._yaw_list[1])+'\t\t\t'+str(self._yaw_list[2])+'\n')
f.write('\n')
time.sleep(0.1)
def _ramp_suspend(self, suspendthrust, seconds, logfile):
thrust = suspendthrust
f = logfile
i=0
while (i<(seconds*10) or self._suspend) and self._power_on:
roll, pitch, yawrate = self._pid(self._roll_list,self._pitch_list)
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
f.write('roll: \t\t'+str(self._roll_list[0])+'\t\t\t'+str(self._roll_list[1])+'\t\t\t'+str(self._roll_list[2])+'\n')
f.write('pitch: \t\t'+str(self._pitch_list[0])+'\t\t\t'+str(self._pitch_list[1])+'\t\t\t'+str(self._pitch_list[2])+'\n')
f.write('yaw: \t\t'+str(self._yaw_list[0])+'\t\t\t'+str(self._yaw_list[1])+'\t\t\t'+str(self._yaw_list[2])+'\n')
f.write('roll_send:'+str(roll)+'\t\t\tpitch_send'+str(pitch))
f.write('\n\n')
i +=1
time.sleep(0.1)
def _ramp_landing(self, inithrust, minthrust, logfile):
thrust = inithrust
thrust_step = 250
f = logfile
while thrust >= minthrust and self._power_on:
roll,pitch,yawrate = self._pid(self._roll_list,self._pitch_list)
thrust -= thrust_step
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
f.write('roll: \t\t'+str(self._roll_list[0])+'\t\t\t'+str(self._roll_list[1])+'\t\t\t'+str(self._roll_list[2])+'\n')
f.write('pitch: \t\t'+str(self._pitch_list[0])+'\t\t\t'+str(self._pitch_list[1])+'\t\t\t'+str(self._pitch_list[2])+'\n')
f.write('yaw: \t\t'+str(self._yaw_list[0])+'\t\t\t'+str(self._yaw_list[1])+'\t\t\t'+str(self._yaw_list[2])+'\n')
f.write('\n')
time.sleep(0.1)
def _pid(self, roll_list = None, pitch_list = None, yawrate_list = None):
# position method:
roll_bias = 0
pitch_bias = 0
roll_kp=1.75 # neg
roll_ki=0
roll_kd=0.15 # pos
pitch_kp=1.75 # neg
pitch_ki=0
pitch_kd=0.1 # pos
roll_now = roll_list[0]
pitch_now = pitch_list[0]
roll_p = roll_now
self.roll_i += roll_now # accumulate
roll_d = roll_now-roll_list[1]
pitch_p = pitch_now
self.pitch_i += pitch_now # accumulate
pitch_d = pitch_now - pitch_list[1]
roll = roll_kp * roll_p + roll_ki * self.roll_i + roll_kd * roll_d
pitch = pitch_kp * pitch_p + pitch_ki * self.pitch_i + pitch_kd * pitch_d
yawrate = 0
self._roll_list[2] = self._roll_list[1]
self._roll_list[1] = roll_now
self._pitch_list[2] = self._pitch_list[1]
self._pitch_list[1] = pitch_now
self._yaw_list[2] = self._yaw_list[1]
self._yaw_list[1] = self._yaw_list[0]
return (roll + roll_bias, pitch + pitch_bias, yawrate)
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"""
self._roll_list[0] = data['stabilizer.roll']
self._pitch_list[0] = data['stabilizer.pitch']
self._yaw_list[0] = data['stabilizer.yaw']
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 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 = 20 # (Hz) Rate of each sending control input
self.statefb_rate = 50 # (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
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.5
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.3
safe_thrs = 0.9
col_muy = 2.0
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
# Get fp using smooth step function
fp[0] = fp_ref[0] * self.step_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0)
fp[1] = fp_ref[1] * self.step_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0)
fp[2] = fp_ref[2] * self.step_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0)
fv[0] = fp_ref[0] * self.step_dot_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0)
fv[1] = fp_ref[1] * self.step_dot_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0)
fv[2] = fp_ref[2] * self.step_dot_fcn(time.time(), timeBeginCtrl + 1.0, timeBeginCtrl + 10.0)
# Get roll, pitch, yaw
roll, pitch, yaw = self.attitude
x, y , z = self.position
vx, vy, vz = self.velocity
# Get eta
eta_px = x - (rot_mat[0][0] * fp[0] + rot_mat[0][1] * fp[1] + rot_mat[0][2] * fp[2]) - coef_init * x_0
eta_py = y - (rot_mat[1][0] * fp[0] + rot_mat[1][1] * fp[1] + rot_mat[1][2] * fp[2]) - coef_init * y_0
eta_pz = z - (rot_mat[2][0] * fp[0] + rot_mat[2][1] * fp[1] + rot_mat[2][2] * fp[2]) - coef_init * z_0
eta_vx = vx - fv[0]
eta_vy = vy - fv[1]
eta_vz = vz - fv[2]
error_z = z_d - eta_pz
z_ctrl_cp = z_ctrl_cp + (error_z + error_z_pre) / 2.0 * timeSampling
error_z_pre = error_z
# Get u tracking
u_x_tr = - gp * delta * (eta_px - x_d)
u_y_tr = - gp * delta * (eta_py - y_d)
u_z_tr = - gp * delta * (eta_pz - z_d)
# Multi range dot
Mtr_dot[0] = (self.Mtr[0] - Mtr_pre[0]) / timeSampling
Mtr_dot[1] = (self.Mtr[1] - Mtr_pre[1]) / timeSampling
Mtr_dot[2] = (self.Mtr[2] - Mtr_pre[2]) / timeSampling
Mtr_dot[3] = (self.Mtr[3] - Mtr_pre[3]) / timeSampling
# Collision avoidance
u_x_c = - self.bump_fcn_dot(self.Mtr[0],col_thrs,safe_thrs,col_muy,col_lda) * Mtr_dot[0] \
+ self.bump_fcn_dot(self.Mtr[1],col_thrs,safe_thrs,col_muy,col_lda) * Mtr_dot[1]
u_y_c = - self.bump_fcn_dot(self.Mtr[2],col_thrs,safe_thrs,col_muy,col_lda) * Mtr_dot[2] \
+ self.bump_fcn_dot(self.Mtr[3],col_thrs,safe_thrs,col_muy,col_lda) * Mtr_dot[3]
u_z_c = self.bump_fcn_dot(self.Mtr[0],col_thrs,safe_thrs,col_muy,col_lda) \
* self.bump_fcn_dot(self.Mtr[1],col_thrs,safe_thrs,col_muy,col_lda)
# Storage multi range information
Mtr_pre = self.Mtr
# Get u (tracking and collision avoidance)
u_x = self.step_fcn(self.Mtr[0],col_thrs,safe_thrs) \
* self.step_fcn(self.Mtr[1],col_thrs,safe_thrs) \
* u_x_tr + u_x_c
u_y = self.step_fcn(self.Mtr[2],col_thrs,safe_thrs) \
* self.step_fcn(self.Mtr[3],col_thrs,safe_thrs) \
* u_y_tr + u_y_c #+ u_x_c
u_z = (self.step_fcn(self.Mtr[0],col_thrs,safe_thrs) \
+ self.step_fcn(self.Mtr[1],col_thrs,safe_thrs))\
* u_z_tr + u_z_c
#u_x = u_x_tr
#u_y = u_y_tr
u_z = u_z_tr
u = [u_x, u_y, u_z]
#print(u_z_c)
x_d_c = u_x_c + x_d
y_d_c = u_y_c + y_d
# Storage data
self.datalog_Pos.write(str(time.time()) + "," +
str(self.position[0]) + "," +
str(self.position[1]) + "," +
str(self.position[2]) + "\n")
# Check whether control is enable or not
if self.isEnabled:
# Get thrust and attitude desired
thrust_d, roll_d, pitch_d = self.output2nd(u, yaw, m, g)
message = ('ref:({}, {}, {})\n'.format(x_d, y_d, z_d) +
'pos:({}, {}, {})\n'.format(x, y, z) +
'att:({}, {}, {})\n'.format(roll, pitch, yaw) +
'control:({}, {}, {})\n'.format(roll_d, pitch_d, thrust_d))
else:
thrust_d, roll_d, pitch_d, yaw_d = (0.0, 0.0, 0.0, 0.0)
self._cf.close_link()
print('Force disconnecting')
# 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_c,y_d_c,0.4,yaw_d)
self._cf.commander.send_velocity_world_setpoint(u_x, u_y, u_z_tr, 0.0)
self.print_at_period(2.0, message)
self.loop_sleep(timeStart)
# End
def bump_fcn(self, mr, threshold, muy):
""" This is bump function"""
if mr > threshold:
mr_bump = 0
else:
mr_bump = (threshold - mr) * (threshold - mr)
mr_bump = mr_bump/(mr + threshold * threshold * 1.0/muy)
return mr_bump
def bump_fcn_dot(self, mr, col_thrs, safe_thrs, muy, lda):
""" This is the derivative of the bump function"""
if mr > col_thrs:
mr_bump_dot = lda * self.step_dot_fcn(mr, col_thrs, safe_thrs)
else:
mr_bump_dot = float(mr) + 2.0 * col_thrs * col_thrs / muy + col_thrs
mr_bump_dot = - mr_bump_dot * (col_thrs - float(mr))
mr_bump_dot = mr_bump_dot / ((float(mr) + col_thrs * col_thrs / muy)*(float(mr) + col_thrs * col_thrs / muy))
return mr_bump_dot
def step_fcn(self, mr, low, up):
""" This is smooth step function """
if mr < low:
step_out = 0
elif mr > low and mr < up:
step_out = (float(mr) - low)/(up - low)
step_out = step_out * step_out
else:
step_out = 1
return step_out
def step_dot_fcn(self, mr, low, up):
""" This is smooth step function """
if mr <= low or mr >= up:
step_out = 0
else:
step_out = 2*(float(mr) - low)/(up - low)
step_out = step_out / (up - low)
return step_out
def output2nd(self, u_out, yaw, m, g):
""" This calculates the thrust force and attitude desired """
# Calculate tau
tau1 = m * (cos(yaw) * u_out[0] + sin(yaw) * u_out[1])
tau2 = m * (-sin(yaw) * u_out[0] + cos(yaw) * u_out[1])
tau3 = m * (u_out[2] + g)
# Calculate thrust and attitude desired
thrust = sqrt(tau1 * tau1 + tau2 * tau2 + tau3 * tau3)
roll_d = asin(-tau2/thrust)
pitch_d = atan2(tau1,tau3)
# thrust in 16bit and angle to degree
thrust_16bit_limit = self.thrust2cmd(thrust)
roll_d = roll_d * 180.0 / self.pi
pitch_d = pitch_d * 180.0 / self.pi
# Saturation of roll and pitch desired
roll_d_limit = self.saturation(roll_d, self.roll_limit)
pitch_d_limit = self.saturation(pitch_d, self.pitch_limit)
return [thrust_16bit_limit, roll_d_limit, pitch_d_limit]
def thrust2cmd(self, thrust):
""" This is able to transform thrust in Newton to command in integer 0-65000
We need to solve the second order polynominal to find
cmd_thrust in integer corresponding to thrust in Newton
thrust = 4 * (a * cmd_thrust ^ 2 + b * cmd_thrust + c)"""
a = 2.130295e-11
#a = 2.230295e-11
b = 1.032633e-6
c = 5.48456e-4
delta = b * b - 4 * a * (c - thrust/4.0)
cmd_thrust = (-b + sqrt(delta))/(2 * a)
cmd_thrust = int(round(cmd_thrust))
cmd_thrust = self.saturation(cmd_thrust, self.thrust_limit)
return cmd_thrust
def saturation(self, value, limits):
""" Saturate a given value within limit interval"""
if value < limits[0]:
value = limits[0]
#print("limit low")
elif value > limits[1]:
value = limits[1]
#print("limit up")
return value
def print_at_period(self, period, message):
""" Prints the message at a given period"""
if (time.time() - period) > self.timePrint:
self.timePrint = time.time()
print(message)
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
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 OWExample:
"""
Simple example listing the 1-wire memories found and lists its contents.
"""
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
self._mems_to_update = 0
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_1W)
self._mems_to_update = len(mems)
print("Found {} 1-wire memories".format(len(mems)))
for m in mems:
print("Updating id={}".format(m.id))
m.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("\tName : {}".format(mem.name))
print("\tVID : 0x{:02X}".format(mem.vid))
print("\tPID : 0x{:02X}".format(mem.pid))
print("\tPins : 0x{:02X}".format(mem.pins))
print("\tElements : ")
for key in mem.elements:
print("\t\t{}={}".format(key, mem.elements[key]))
self._mems_to_update -= 1
if self._mems_to_update == 0:
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 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()
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 EEPROMExample:
"""
Simple example listing the EEPROMs found and lists its contents.
"""
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_1W)
print('Found {} 1-wire memories'.format(len(mems)))
if len(mems) > 0:
print('Writing test configuration to'
' memory {}'.format(mems[0].id))
mems[0].vid = 0xBC
mems[0].pid = 0xFF
board_name_id = OWElement.element_mapping[1]
board_rev_id = OWElement.element_mapping[2]
mems[0].elements[board_name_id] = 'Test board'
mems[0].elements[board_rev_id] = 'A'
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('\tName : {}'.format(mem.name))
print('\tVID : 0x{:02X}'.format(mem.vid))
print('\tPID : 0x{:02X}'.format(mem.pid))
print('\tPins : 0x{:02X}'.format(mem.pins))
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 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 CrazyflieROS:
Disconnected = 0
Connecting = 1
Connected = 2
"""Wrapper between ROS and Crazyflie SDK"""
def __init__(self, link_uri, tf_prefix, roll_trim, pitch_trim, enable_logging):
self.link_uri = link_uri
self.tf_prefix = tf_prefix
self.roll_trim = roll_trim
self.pitch_trim = pitch_trim
self.enable_logging = enable_logging
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.link_quality_updated.add_callback(self._link_quality_updated)
self._cmdVel = Twist()
self._subCmdVel = rospy.Subscriber(tf_prefix + "/cmd_vel", Twist, self._cmdVelChanged)
self._pubImu = rospy.Publisher(tf_prefix + "/imu", Imu, queue_size=10)
self._pubTemp = rospy.Publisher(tf_prefix + "/temperature", Temperature, queue_size=10)
self._pubMag = rospy.Publisher(tf_prefix + "/magnetic_field", MagneticField, queue_size=10)
self._pubPressure = rospy.Publisher(tf_prefix + "/pressure", Float32, queue_size=10)
self._pubBattery = rospy.Publisher(tf_prefix + "/battery", Float32, queue_size=10)
self._state = CrazyflieROS.Disconnected
rospy.Service(tf_prefix + "/update_params", UpdateParams, self._update_params)
rospy.Service(tf_prefix + "/emergency", Empty, self._emergency)
self._isEmergency = False
Thread(target=self._update).start()
def _try_to_connect(self):
rospy.loginfo("Connecting to %s" % self.link_uri)
self._state = CrazyflieROS.Connecting
self._cf.open_link(self.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."""
rospy.loginfo("Connected to %s" % link_uri)
self._state = CrazyflieROS.Connected
if self.enable_logging:
self._lg_imu = LogConfig(name="IMU", period_in_ms=10)
self._lg_imu.add_variable("acc.x", "float")
self._lg_imu.add_variable("acc.y", "float")
self._lg_imu.add_variable("acc.z", "float")
self._lg_imu.add_variable("gyro.x", "float")
self._lg_imu.add_variable("gyro.y", "float")
self._lg_imu.add_variable("gyro.z", "float")
self._cf.log.add_config(self._lg_imu)
if self._lg_imu.valid:
# This callback will receive the data
self._lg_imu.data_received_cb.add_callback(self._log_data_imu)
# This callback will be called on errors
self._lg_imu.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_imu.start()
else:
rospy.logfatal("Could not add logconfig since some variables are not in TOC")
self._lg_log2 = LogConfig(name="LOG2", period_in_ms=100)
self._lg_log2.add_variable("mag.x", "float")
self._lg_log2.add_variable("mag.y", "float")
self._lg_log2.add_variable("mag.z", "float")
self._lg_log2.add_variable("baro.temp", "float")
self._lg_log2.add_variable("baro.pressure", "float")
self._lg_log2.add_variable("pm.vbat", "float")
# self._lg_log2.add_variable("pm.state", "uint8_t")
self._cf.log.add_config(self._lg_log2)
if self._lg_log2.valid:
# This callback will receive the data
self._lg_log2.data_received_cb.add_callback(self._log_data_log2)
# This callback will be called on errors
self._lg_log2.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_log2.start()
else:
rospy.logfatal("Could not add logconfig since some variables are not in TOC")
# self._lg_log3 = LogConfig(name="LOG3", period_in_ms=100)
# self._lg_log3.add_variable("motor.m1", "int32_t")
# self._lg_log3.add_variable("motor.m2", "int32_t")
# self._lg_log3.add_variable("motor.m3", "int32_t")
# self._lg_log3.add_variable("motor.m4", "int32_t")
# self._lg_log3.add_variable("stabalizer.pitch", "float")
# self._lg_log3.add_variable("stabalizer.roll", "float")
# self._lg_log3.add_variable("stabalizer.thrust", "uint16_")
# self._lg_log3.add_variable("stabalizer.yaw", "float")
#
# self._cf.log.add_config(self._lg_log3)
#if self._lg_log3.valid:
# # This callback will receive the data
# self._lg_log3.data_received_cb.add_callback(self._log_data_log3)
# # This callback will be called on errors
# self._lg_log3.error_cb.add_callback(self._log_error)
# # Start the logging
# self._lg_log3.start()
#else:
# rospy.logfatal("Could not add logconfig since some variables are not in TOC")
p_toc = self._cf.param.toc.toc
for group in p_toc.keys():
self._cf.param.add_update_callback(group=group, name=None, cb=self._param_callback)
for name in p_toc[group].keys():
ros_param = "/{}/{}/{}".format(self.tf_prefix, group, name)
cf_param = "{}.{}".format(group, name)
if rospy.has_param(ros_param):
self._cf.param.set_value(cf_param, rospy.get_param(ros_param))
else:
self._cf.param.request_param_update(cf_param)
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
rospy.logfatal("Connection to %s failed: %s" % (link_uri, msg))
self._state = CrazyflieROS.Disconnected
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
rospy.logfatal("Connection to %s lost: %s" % (link_uri, msg))
self._state = CrazyflieROS.Disconnected
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
rospy.logfatal("Disconnected from %s" % link_uri)
self._state = CrazyflieROS.Disconnected
def _link_quality_updated(self, percentage):
"""Called when the link driver updates the link quality measurement"""
if percentage < 80:
rospy.logwarn("Connection quality is: %f" % (percentage))
def _log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
rospy.logfatal("Error when logging %s: %s" % (logconf.name, msg))
def _log_data_imu(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
msg = Imu()
# ToDo: it would be better to convert from timestamp to rospy time
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.tf_prefix + "/base_link"
msg.orientation_covariance[0] = -1 # orientation not supported
# measured in deg/s; need to convert to rad/s
msg.angular_velocity.x = math.radians(data["gyro.x"])
msg.angular_velocity.y = math.radians(data["gyro.y"])
msg.angular_velocity.z = math.radians(data["gyro.z"])
# measured in mG; need to convert to m/s^2
msg.linear_acceleration.x = data["acc.x"] * 9.81
msg.linear_acceleration.y = data["acc.y"] * 9.81
msg.linear_acceleration.z = data["acc.z"] * 9.81
self._pubImu.publish(msg)
#print "[%d][%s]: %s" % (timestamp, logconf.name, data)
def _log_data_log2(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
msg = Temperature()
# ToDo: it would be better to convert from timestamp to rospy time
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.tf_prefix + "/base_link"
# measured in degC
msg.temperature = data["baro.temp"]
self._pubTemp.publish(msg)
# ToDo: it would be better to convert from timestamp to rospy time
msg = MagneticField()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.tf_prefix + "/base_link"
# measured in Tesla
msg.magnetic_field.x = data["mag.x"]
msg.magnetic_field.y = data["mag.y"]
msg.magnetic_field.z = data["mag.z"]
self._pubMag.publish(msg)
msg = Float32()
# hPa (=mbar)
msg.data = data["baro.pressure"]
self._pubPressure.publish(msg)
# V
msg.data = data["pm.vbat"]
self._pubBattery.publish(msg)
def _param_callback(self, name, value):
ros_param = "{}/{}".format(self.tf_prefix, name.replace(".", "/"))
rospy.set_param(ros_param, value)
def _update_params(self, req):
rospy.loginfo("Update parameters %s" % (str(req.params)))
for param in req.params:
ros_param = "/{}/{}".format(self.tf_prefix, param)
cf_param = param.replace("/", ".")
print(cf_param)
#if rospy.has_param(ros_param):
self._cf.param.set_value(cf_param, str(rospy.get_param(ros_param)))
return UpdateParamsResponse()
def _emergency(self, req):
rospy.logfatal("Emergency requested!")
self._isEmergency = True
return EmptyResponse()
def _send_setpoint(self):
roll = self._cmdVel.linear.y + self.roll_trim
pitch = self._cmdVel.linear.x + self.pitch_trim
yawrate = self._cmdVel.angular.z
thrust = min(max(0, int(self._cmdVel.linear.z)), 60000)
#print(roll, pitch, yawrate, thrust)
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
def _cmdVelChanged(self, data):
self._cmdVel = data
if not self._isEmergency:
self._send_setpoint()
def _update(self):
while not rospy.is_shutdown():
if self._isEmergency:
break
if self._state == CrazyflieROS.Disconnected:
self._try_to_connect()
elif self._state == CrazyflieROS.Connected:
# Crazyflie will shut down if we don't send any command for 500ms
# Hence, make sure that we don't wait too long
# However, if there is no connection anymore, we try to get the flie down
if self._subCmdVel.get_num_connections() > 0:
self._send_setpoint()
else:
self._cmdVel = Twist()
self._send_setpoint()
rospy.sleep(0.2)
else:
rospy.sleep(0.5)
for i in range(0, 100):
self._cf.commander.send_setpoint(0, 0, 0, 0)
rospy.sleep(0.01)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
rospy.sleep(0.1)
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):
""" 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 formationControl(threading.Thread):
""" A controller thread, taking references and mearsurements from the UAV
computing a control input"""
def __init__(self, link_uri, delta_ref, formation_fp, order_Cf2, num_Cf2):
threading.Thread.__init__(self)
#self.link_uri = link_uri
self.daemon = True
self.timePrint = 0.0
# 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.pos_other = ([0.0] * 3) * 1
self.formation_fp = formation_fp # Formation shape for each agent
# Control setting
self.isEnabled = True
self.printDis = True
# System parameter
self.pi = 3.14159265359
self.delta_ref = delta_ref
self.order_Cf2 = order_Cf2
self.num_Cf2 = num_Cf2
# Open text file to storage data
self.myPos_log = open("log_data/Pos_log" + str(self.order_Cf2), "w")
self.myInput_log = open("log_data/Input_log" + str(self.order_Cf2), "w")
def _run_controller(self):
""" Main control loop
# System parameters"""
timeSampling = 1.0/float(self.rate) # [s] sampling time
delta = 1 # [] initial time, all Crazyflie lifted to 0.3m
# Control Parameters
# Reference parameter
v0 = [0.0, 0.0, 0.0] # [m/s] reference velocity
fp = [0.0, 0.0, 0.0] # [m] the formation shape
fv = [0.0, 0.0, 0.0] # [m/s] the velocity formation shape
sh_start = 0.0
sh_end = 10.0
# Collision parameters
col_thrs = 0.5
safe_thrs = 0.9
col_muy = 0.5
col_lda = 0.0
#dis_col = [0.0] * (self.num_Cf2 - 1)
# Set the current reference to the current positional estimate
time.sleep(3)
# 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 > 4.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 = [0.0, 0.0, 0.0] # initial time
pos_other = ([0.0] * 3) * 1
# 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.0:
v0 = [0.0, 0.0, 0.0]
delta = self.delta_ref
if time.time() > timeBeginCtrl + 20.0:
v0 = [0.02, -0.02, 0.0]
if time.time() > timeBeginCtrl + 35.0:
v0 = [-0.02, 0.02, 0.0]
if time.time() > timeBeginCtrl + 50.0:
v0 = [0.0, 0.0, 0.0]
if time.time() > timeBeginCtrl + 60.0:
self._cf.commander.send_setpoint(0, 0, 0, 40000)
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
# Get roll, pitch, yaw
roll, pitch, yaw = self.attitude
x, y , z = self.position
vx, vy, vz = self.velocity
# Get eta
#eta_px = x - fp[0]
#eta_py = y - fp[1]
#eta_pz = z - fp[2]
# A loop computes a potential function producing repulsive force
u_x_c = 0
u_y_c = 0
u_z_c = 0
count_near = 0
for i in range(0, self.num_Cf2 - 1):
# Get position from others
pos_other = self.pos_other[i][0:3]
# Collision avoidance
e_col_x = x - pos_other[0]
e_col_y = y - pos_other[1]
e_col_z = z - pos_other[2]
dis_col = sqrt(e_col_x*e_col_x + e_col_y*e_col_y + e_col_z*e_col_z)
# collion-free force
repul_force = self.bump_fcn_dot(dis_col,col_thrs,safe_thrs,col_muy,col_lda)
u_x_c = u_x_c - repul_force * 2 * e_col_x #* vx
u_y_c = u_y_c - repul_force * 2 * e_col_y #* vy
#u_z_c = -repul_force * 2 * e_col_z #* vz
u_z_c = u_z_c + u_x_c + u_y_c
if repul_force != 0.0:
count_near = count_near + 1
##
u_z_c = self.saturation(u_z_c, [-0.2, 0.2])
# More than 2 other Cf2 near this Cf2
if count_near > 1:
u_z_c = 0.2 * (count_near - 1.0)
# Setpoint
x_d_c = x_0 + (x_d - x_0 + self.formation_fp[0]) * self.step_fcn(time.time(), timeBeginCtrl + sh_start, timeBeginCtrl + sh_end) + u_x_c
y_d_c = y_0 + (y_d - y_0 + self.formation_fp[1]) * self.step_fcn(time.time(), timeBeginCtrl + sh_start, timeBeginCtrl + sh_end) + u_y_c
# Send set point
self._cf.commander.send_position_setpoint(x_d_c,y_d_c,0.4+u_z_c,yaw_d)
#if self.order_Cf2 == 0:
# self._cf.commander.send_position_setpoint(x_d_c,y_d_c,0.4,yaw_d)
#else:
# self._cf.commander.send_setpoint(0, 0, 0, 20000)
##
message = ('ref:({}, {}, {})\n'.format(x_d_c, y_d_c, z_d) +
'pos:({}, {}, {})\n'.format(x, y, z) +
'pos_other:({}, {}, {})\n'.format(pos_other[0], pos_other[1], pos_other[2]) +
'dis:({})\n'.format(dis_col))
message1 = ('rep_force :({}, {}, {})\n'.format(u_x_c, u_y_c, u_z_c))
if self.order_Cf2 == 0:
self.print_at_period(0.5, message1)
# Storage data
self.myPos_log.write(str(time.time()) + "," +
str(x) + "," +
str(y) + "," +
str(z) + "\n")
#self.myInput_log.write(str(time.time()) + "," +
# str(u_x) + "," +
# str(u_y) + "," +
# str(u_z) + "\n")
self.loop_sleep(timeStart)
# End
def saturation(self, value, limits):
""" Saturate a given value within limit interval"""
if value < limits[0]:
value = limits[0]
#print("limit low")
elif value > limits[1]:
value = limits[1]
#print("limit up")
return value
def bump_fcn(self, mr, threshold, muy):
""" This is bump function"""
if mr > threshold:
mr_bump = 0
else:
mr_bump = (threshold - mr) * (threshold - mr)
mr_bump = mr_bump/(mr + threshold * threshold * 1.0/muy)
return mr_bump
def bump_fcn_dot(self, mr, col_thrs, safe_thrs, muy, lda):
""" This is the derivative of the bump function"""
if mr > col_thrs:
mr_bump_dot = lda * self.step_dot_fcn(mr, col_thrs, safe_thrs)
else:
mr_bump_dot = float(mr) + 2.0 * col_thrs * col_thrs / muy + col_thrs
mr_bump_dot = - mr_bump_dot * (col_thrs - float(mr))
mr_bump_dot = mr_bump_dot / ((float(mr) + col_thrs * col_thrs / muy)*(float(mr) + col_thrs * col_thrs / muy))
return mr_bump_dot
def step_fcn(self, mr, low, up):
""" This is smooth step function """
if mr <= low:
step_out = 0
elif mr > low and mr < up:
step_out = (float(mr) - low)/(up - low)
step_out = step_out * step_out
else:
step_out = 1
return step_out
def step_dot_fcn(self, mr, low, up):
""" This is smooth step function """
if mr <= low or mr >= up:
step_out = 0
else:
step_out = 2*(float(mr) - low)/(up - low)
step_out = step_out / (up - low)
return step_out
def print_at_period(self, period, message):
""" Prints the message at a given period"""
if (time.time() - period) > self.timePrint:
self.timePrint = time.time()
print(message)
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)
# 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')
# Invoke logged states
self._cf.log.add_config(logPos)
self._cf.log.add_config(logVel)
self._cf.log.add_config(logAtt)
# Start invoking logged states
if logPos.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()
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.myPos_log.write(str(time.time()) + "," +
# str(self.position[0]) + "," +
# str(self.position[1]) + "," +
# str(self.position[2]) + "\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
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 subscribe_other_infor(self, pos_other):
""" This function updates the information including eta_p and eta_v from
other Crazyflies """
self.pos_other = pos_other
def get_publish_infor(self):
""" This calback take the information of this Crazyflie"""
return self.position
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 UAVController():
def __init__(self, targetURI=None):
"""
Function: __init__
Purpose: Initialize all necessary UAV functionality
Inputs: none
Outputs: none
"""
cflib.crtp.init_drivers()
#self.FD = open("logFile.txt", "w")
self.timeout = True
self.available = []
self.UAV = Crazyflie()
self.param = None
self.airborne = False
self._recentDataPacket = None
self._receivingDataPacket = False
#Attempt to locate UAV by scanning available interface
for _ in range(0, 500):
if (len(self.available) > 0):
self.timeout = False
break #If a UAV is found via scanning, break out of this loop
else:
self.available = cflib.crtp.scan_interfaces()
if (len(self.available) > 0):
if (targetURI != None):
for i in range(len(self.available)):
if (self.available[i][0] == targetURI):
self.UAV.open_link(self.available[i][0])
self.connectedToTargetUAV = True
else:
self.connectedToTargetUAV = False
else:
self.UAV.open_link(self.available[0][0])
while (self.UAV.is_connected() == False):
time.sleep(0.1)
self.MC = MotionCommander(self.UAV)
#Create desired logging parameters
self.UAVLogConfig = LogConfig(name="UAVLog", period_in_ms=100)
#self.UAVLogConfig.add_variable('pm.batteryLevel', 'float')
self.UAVLogConfig.add_variable('pm.vbat', 'float')
self.UAVLogConfig.add_variable('pm.batteryLevel', 'float')
#self.UAVLogConfig.add_variable('stateEstimate.x', 'float')
#self.UAVLogConfig.add_variable('stateEstimate.y', 'float')
self.UAVLogConfig.add_variable('stateEstimate.z', 'float')
self.UAVLogConfig.add_variable('pm.chargeCurrent', 'float')
self.UAVLogConfig.add_variable('pm.extCurr', 'float')
self.UAVLogConfig.add_variable('pwm.m1_pwm', 'float')
#self.UAVLogConfig.add_variable('baro.pressure', 'float')
#self.UAVLogConfig.add_variable('extrx.thrust', 'float')
#Add more variables here for logging as desired
self.UAV.log.add_config(self.UAVLogConfig)
if (self.UAVLogConfig.valid):
self.UAVLogConfig.data_received_cb.add_callback(
self._getUAVDataPacket)
self.UAVLogConfig.start()
else:
logger.warning("Could not setup log configuration")
self._startTime = time.time() #For testing purposes
self._trialRun = 0
#End of function
def done(self):
"""
Function: done
Purpose: Close connection to UAV to terminate all threads running
Inputs: none
Outputs: none
"""
self.UAVLogConfig.stop()
self.UAV.close_link()
self.airborne = False
return
def launch(self):
"""
Function: launch
Purpose: Instruct the UAV to takeoff from current position to the default height
Inputs: none
Outputs: none
"""
self.airborne = True
self.MC.take_off()
#End of function
return
def land(self):
"""
Function: launch
Purpose: Instruct the UAV to land on the ground at current position
Inputs: none
Outputs: none
"""
self.MC.land()
return
def move(self, distanceX, distanceY, distanceZ, velocity):
"""
Function: move
Purpose: A wrapper function to instruct an UAV to move <x, y, z> distance from current point
Inputs: distance - a floating point value distance in meters
velocity - a floating point value velocity in meters per second
Outputs: none
"""
if (self.airborne == False):
self.launch()
self.MC.move_distance(distanceX, distanceY, distanceZ, velocity)
#End of function
return
def rotate(self, degree):
"""
Function: rotate
Purpose: A wrapper function to instruct an UAV to rotate
Inputs: degree - a floating point value in degrees
Outputs: none
"""
if (self.airborne == False):
self.launch()
if (degree < 0):
print("UC: rotate - Going Right")
locDeg = 0
#self.MC.turn_right(abs(degree))
for _ in range(1, int(abs(degree) / 1)):
locDeg += 1
self.MC.turn_right(1)
self.MC.turn_right(abs(degree) - locDeg)
else:
print("UC: rotate - Going Left")
self.MC.turn_left(abs(degree))
#Delay by 1 seclond, to allow for total rotation time
time.sleep(1)
return
def getBatteryLevel(self):
"""
Function: getBatteryLevel
Purpose: A function that reads the UAV battery level from a IOStream
Inputs: none
Outputs: none
Description:
"""
retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
retVal = self._recentDataPacket["pm.vbat"]
return retVal
def getHeight(self):
"""
Function: getCurrentHeight
Purpose: A function that reads the UAV height from a IOStream
Inputs: none
Outputs: none
Description:
"""
retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
retVal = self._recentDataPacket["stateEstimate.z"]
return retVal
def isCharging(self):
"""
Function: getCurrentHeight
Purpose: A function that reads the UAV height from a IOStream
Inputs: none
Outputs: none
Description:
"""
retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
retVal = self._recentDataPacket["pm.chargeCurrent"]
return retVal
def _getUAVDataPacket(self, ident, data, logconfig):
"""
Function: getUAVDataPacket
Purpose: Process a data packet received from the UAV
Inputs: ident -
data -
logconfig -
Outputs: None
Description: A user should never call this function.
"""
self._receivingDataPacket = True
self._recentDataPacket = data
self._receivingDataPacket = False
def appendToLogFile(self):
"""
Function: appendToLogFile
Purpose: append log variables to log file 'log.txt.'
Inputs: none
Outputs: none
Description:
"""
#retVal = None
if (self._recentDataPacket != None
and self._receivingDataPacket == False):
current = self._recentDataPacket["pm.chargeCurrent"]
extcurrent = self._recentDataPacket["pm.extCurr"]
voltage = self._recentDataPacket["pm.vbat"]
bat_level = self._recentDataPacket["pm.batteryLevel"]
height = self._recentDataPacket["stateEstimate.z"]
#x = self._recentDataPacket["stateEstimate.y"]
#y = self._recentDataPacket["stateEstimate.x"]
m1_pwm = self._recentDataPacket["pwm.m1_pwm"]
#extrx_thrust = self._recentDataPacket["extrx.thrust"]
#baro_pressure = self._recentDataPacket["baro.pressure"]
with open('log2.txt', 'a') as file:
#print(batlevel)
file.write(str(datetime.now()) + '\n')
file.write('bat_voltage: ' + str(voltage) + '\n')
file.write('bat_level: ' + str(bat_level) + '\n')
file.write('crg_current: ' + str(current) + '\n')
file.write('ext_current: ' + str(extcurrent) + '\n')
file.write('height: ' + str(height) + '\n')
#file.write('x: ' + str(x) + '\n')
#file.write('y: ' + str(y) + '\n')
file.write('m1_pwm: ' + str(m1_pwm) + '\n')
