class TestFlight:
def __init__(self):
"""
Initialize the quad copter
"""
# Log file for Accelerometer
self.f1 = open('Accelerometer_log.txt', 'w')
# Log file for Pitch Roll Yaw
self.f2 = open('Stabalizer_log.txt', 'w')
# Log file for altitude
self.f3 = open('Barometer_log.txt', 'w')
# write data into the log files
self.f1.write('Accelerometer log\n {date} {acc.x} {acc.y} {acc.z}\n')
self.f2.write('Stabilizer log\n{date} {Roll} {Pitch} {Yaw} {Thrust}\n')
self.f3.write('Barometer log\n {data} {ASL}\n')
# get the Unix time
self.starttime = time.time()*1000.0
self.date = time.time()*1000.0 - self.starttime
# Initialize the crazyflie and get the drivers ready
self.crazyflie = cflib.crazyflie.Crazyflie()
print 'Initializing drivers'
cflib.crtp.init_drivers()
# Start scanning available devices
print 'Searching for available devices'
available = cflib.crtp.scan_interfaces()
radio = False
for i in available:
# Connect to the first device of the type 'radio'
if 'radio' in i[0]:
radio = True
dev = i[0]
print 'Connecting to interface with URI [{0}] and name {1}'.format(i[0], i[1])
self.crazyflie.open_link("radio://0/80/250K")
## radio://0/80/250K is most well connected radio frequency
# Heungseok Park, 4.9.2015
#self.crazyflie.open_link(dev)
break
if not radio:
print 'No quadcopter detected. Try to connect again.'
exit(-1)
# Set up the callback when connected
self.crazyflie.connected.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
"""
Set the loggers
"""
"""
# Log stabilizer
self.logStab = LogConfig("Stabalizer", 200)
self.logStab.add_variable("stabilizer.roll", "float")
self.logStab.add_variable("stabilizer.pitch", "float")
self.logStab.add_variable("stabilizer.yaw", "float")
self.logStab.add_variable("stabilizer.thrust", "uint16_t")
self.crazyflie.log.add_config(self.logStab)
if self.logStab.valid:
self.logStab.data_received_cb.add_callback(self.print_stab_data)
self.logStab.start()
else:
print 'Could not setup log configuration for stabilizer after connection!'
"""
# Log barometer
# we use only barometer value(ASL Value) to control altitude
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable("baro.aslLong", "float")
self.crazyflie.log.add_config(self.logBaro)
if self.logBaro.valid:
self.logBaro.data_received_cb.add_callback(self.print_baro_data)
self.logBaro.start()
else:
print 'Could not setup log configuration for barometer after connection!'
"""
# Log Accelerometer
self.logAccel = LogConfig("Accel",200)
self.logAccel.add_variable("acc.x", "float")
self.logAccel.add_variable("acc.y", "float")
self.logAccel.add_variable("acc.z", "float")
self.crazyflie.log.add_config(self.logAccel)
if self.logAccel.valid:
self.logAccel.data_received_cb.add_callback(self.print_accel_data)
self.logAccel.start()
else:
print 'Could not setup log configuration for accelerometer after connection!'
"""
# Start another thread and doing control function call
print "log for debugging: before start increasing_step"
# Thread(target=self.increasing_step).start()
# Thread(target=self.recursive_step).start()
Thread(target=self.init_alt).start()
def print_baro_data(self, ident, data, logconfig):
# Output the Barometer data
#logging.info("Id={0}, Barometer: asl={1:.4f}".format(ident, data["baro.aslLong"]))
# global variable that holds current altitude
global current_alt
current_alt = data["baro.aslLong"]
# system output the time and the altitude, each id represents a time slice in Unix
date = time.time()*1000.0 - self.starttime
sys.stdout.write('Id={0}, Baro: baro.aslLong{1:.4f}\r\n'.format(ident, data["baro.aslLong"]))
self.f3.write('{} {}\n'.format(date, data["baro.aslLong"]))
pass
def print_stab_data(self, ident, data, logconfig):
# Output the stablizer data (roll pith yaw)
sys.stdout.write('Id={0}, Stabilizer: Roll={1:.2f}, Pitch={2:.2f}, Yaw={3:.2f}, Thrust={4:.2f}\r'.format(ident, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"]))
# print('Id={0}, Stabilizer: Roll={1:.2f}, Pitch={2:.2f}, Yaw={3:.2f}, Thrust={4:.2f}\r'.format(ident, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"]))
self.f2.write('{} {} {} {} {}\n'.format(self.date, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"]))
def print_accel_data(self, ident, data, logconfig):
# Output the accelerometer data
# global variables that holds x,y,z value
global accelvaluesX
global accelvaluesY
global accelvaluesZ
sys.stdout.write('Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}\r".format(ident, data["acc.x"], data["acc.y"], data["acc.z"]))')
#print("Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}\n".format(ident, data["acc.x"], data["acc.y"], data["acc.z"]))
self.f1.write('{} {} {} {}\n'.format(self.date, data["acc.x"], data["acc.y"], data["acc.z"]))
#sys.stdout.write("Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}\r".format(ident, data["acc.x"], data["acc.y"], data["acc.z"]))
#date = time.time()*1000.0 - self.starttime
#small_array = []
#small_array.append(date)
#small_array.append(data["acc.x"])
#small_array.append(data["acc.y"])
#small_array.append(data["acc.z"])
#accelvaluesX.append(small_array_x)
#print(accelvaluesX)
#print(accelvaluesY)
#print(accelvaluesZ)
def init_alt(self):
#global current_alt
global target_alt
global current_alt
current_temp = current_alt
target_temp = target_alt
#print "- current_temp" + current_temp
#print "- target_temp" + target_temp
# If the current < target the thrust up gain altitude
# round function change float value to int
if(round(current_temp) < target_temp):
sys.stdout.write("Current alt is lower than target value, Let's go up!\r\n")
self.crazyflie.commander.send_setpoint(0, 0, 0, 43000)
#for bandwidth reason, the command need to be delayed
time.sleep(0.5)
return self.init_alt()
# If the current > target the thrust down lose altitude
elif(round(current_temp) > target_temp):
sys.stdout.write("Current alt is higher than target value, Let's go down!\r\n")
self.crazyflie.commander.send_setpoint(0, 0, 0, 32000)
#for bandwidth reason, the command need to be delayed
time.sleep(0.2)
return self.init_alt()
# If the current = target then hold the altitude by using the build-in function althold
elif(round(current_temp) == target_temp):
sys.stdout.write("Now, current and target altitude is same, Let's hover!\r\n")
self.crazyflie.param.set_value("flightmode.althold", "True")
#the althold will last 4 second and then we set the thrust to 32767 which is the value that will hover
self.crazyflie.commander.send_setpoint(0, 0, 0, 32767)
def recursive_step(self, altc, altt):
# this function pass the crazyflie current altitude as well as target altitude
# it recursivly call it self in 3 situations
# Post condition; the drone reach to the target altitude and hover
# Temp variables to hold the parameter
current_temp = altc["baro.aslLong"]
target_temp = altt
# If the current < target the thrust up gain altitude
if(current_temp < target_temp):
sys.stdout.write("Current alt is lower than target value, Let's go up!\r\n")
self.crazyflie.commander.send_setpoint(0, 0, 0, 40000)
current_temp = altc["baro.aslLong"]
return self.recursive_step(current_temp, target_temp)
# If the current > target the thrust down lose altitude
elif(current_temp > target_temp):
sys.stdout.write("Currnet alt is higher than target value, Let's go down!\r\n")
self.crazyflie.commander.send_setpoint(0, 0, 0, 30000)
current_temp= altc["baro.aslLong"]
return self.recursive_step(current_temp, target_temp)
# If the current = target then hold the altitude by using the build-in function althold
elif(current_temp == target_temp):
sys.stdout.write("Now, current and target altitude is same, Let's hover!\r\n")
self.crazyflie.param.set_value("flightmode.althold", "True")
return
def increasing_step(self):
# This function reads the key, and different key input indicates different output
# If you use global var, you need to modify global copy
global key
global accelvaluesX
global accelvaluesY
global accelvaluesZ
global current_alt
# Debug for the current altitude
print(current_alt) # now, this will print 0
# (blades start to rotate after 10000
#initialize the var
# Thrust init
start_thrust = 43000
min_thrust = 10000
max_thrust = 60000
thrust_increment = 3000
# Flag is for the altitude hold mode
flag = True
# Roll init
start_roll = 0
roll_increment = 30
min_roll = -50
max_roll = 50
# Pitch init
start_pitch = 0
pitch_increment = 30
min_pitch = -50
max_pitch = 50
# Yaw init
start_yaw = 0
yaw_increment = 30
min_yaw = -200
max_yaw = 200
stop_moving_count = 0
# Target init
pitch = start_pitch
roll = start_roll
thrust = start_thrust
yaw = start_yaw
#unlock the thrust protection
self.crazyflie.commander.send_setpoint(0,0,0,0)
# Start the keyread thread
keyreader = KeyReaderThread()
keyreader.start()
sys.stdout.write('\r\nCrazyflie Status\r\n')
sys.stdout.write('================\r\n')
sys.stdout.write("Use 'w' and 's' for the thrust, 'a' and 'd' for yaw, 'i' and 'k' for pitch and 'j' and 'l' for roll. Stop flying with 'q'. Exit with 'e'.\r\n")
#g = Gnuplot.Gnuplot(debug=1)
#g.title('A simple example') # (optional)
#g('set data style line') # give gnuplot an arbitrary command
# Plot a list of (x, y) pairs (tuples or a numpy array would
# also be OK):
#g.plot(accelvaluesX)
# key e is to exit the program
while key != "e":
# key q is to kill the drone
if key == 'q':
#thrust = pitch = roll = yaw = 0
print "killing the drone"
thrust = 0
pitch = 0
roll = 0
yaw = 0
# key w is to increase the thrust
elif key == 'w' and (thrust + thrust_increment <= max_thrust):
thrust += thrust_increment
print "thrust: "
print thrust
# key s is to decrease the thrust
elif key == 's' and (thrust - thrust_increment >= min_thrust):
thrust -= thrust_increment
print "thrust: "
print thrust
# key d is to increase the yaw
elif key == 'd' and (yaw + yaw_increment <= max_yaw):
yaw += yaw_increment
stop_moving_count = 0
# key a is to decrease the yaw
elif key == 'a' and (yaw - yaw_increment >= min_yaw):
yaw -= yaw_increment
stop_moving_count = 0
# key l is to increase the roll
elif key == 'l' and (roll + roll_increment <= max_roll):
roll += roll_increment
stop_moving_count = 0
# key j is to decrease the roll
elif key == 'j' and (roll - roll_increment >= min_roll):
roll -= roll_increment
stop_moving_count = 0
# key i is to increase the pitch
elif key == 'i' and (pitch + pitch_increment <= max_pitch):
pitch += pitch_increment
stop_moving_count = 0
# key k is to decrease the pitch
elif key == 'k' and (pitch - pitch_increment >= min_pitch):
pitch -= pitch_increment
stop_moving_count = 0
# 'h' is altitude hold mode
elif key == 'h':
# flag is initialized as true
if flag == True:
self.crazyflie.param.set_value("flightmode.althold", "True")
sys.stdout.write("althold mode\r\n")
print "thrust: "
print thrust
flag = False
else:
self.crazyflie.param.set_value("flightmode.althold", "False")
sys.stdout.write("standard mode\r\n")
print "thrust: "
print thrust
flag = True
#elif key == 'x':
# if the user did not input the keys listed then it count until 40
# then kill the drone
elif key == '':
if stop_moving_count >= 40:
pitch = 0
roll = 0
yaw = 0
else:
stop_moving_count += 1
else:
pass
key = ''
self.crazyflie.commander.send_setpoint(roll, pitch, yaw, thrust)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
self.crazyflie.commander.send_setpoint(0,0,0,0)
self.crazyflie.close_link()
class Quad(threading.Thread):
"""
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 """
threading.Thread.__init__(self)
# 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
Thread(target=self._control).start()
Thread(target=self._input).start()
########################################################################
# Set up Stabilizer Logger
########################################################################
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=50)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._log_stab_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since some variables are not in TOC")
# ########################################################################
# # Set up Acc Logger
# ########################################################################
# self._lg_acc = LogConfig(name="Acc", period_in_ms=50)
# self._lg_acc.add_variable("acc.x", "float")
# self._lg_acc.add_variable("acc.y", "float")
# self._lg_acc.add_variable("acc.z", "float")
# self._cf.log.add_config(self._lg_acc)
# if self._lg_acc.valid:
# # This callback will receive the data
# self._lg_acc.data_received_cb.add_callback(self._log_acc_data)
# # This callback will be called on errors
# self._lg_acc.error_cb.add_callback(self._log_error)
# # Start the logging
# self._lg_acc.start()
# else:
# print("Could not add logconfig since some variables are not in TOC")
# ########################################################################
# # Set up Gyro Logger
# ########################################################################
# self._lg_gyro = LogConfig(name="Gyro", period_in_ms=50)
# self._lg_gyro.add_variable("gyro.x", "float")
# self._lg_gyro.add_variable("gyro.y", "float")
# self._lg_gyro.add_variable("gyro.z", "float")
# self._cf.log.add_config(self._lg_gyro)
# if self._lg_gyro.valid:
# # This callback will receive the data
# self._lg_gyro.data_received_cb.add_callback(self._log_gyro_data)
# # This callback will be called on errors
# self._lg_gyro.error_cb.add_callback(self._log_error)
# # Start the logging
# self._lg_gyro.start()
# else:
# print("Could not add logconfig since some variables are not in TOC")
# ########################################################################
# # Set up Baro Logger
# ########################################################################
self._lg_baro = LogConfig(name="Baro", period_in_ms=50)
self._lg_baro.add_variable("baro.aslLong", "float")
self._cf.log.add_config(self._lg_baro)
if self._lg_baro.valid:
# This callback will receive the data
self._lg_baro.data_received_cb.add_callback(self._log_baro_data)
# This callback will be called on errors
self._lg_baro.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_baro.start()
else:
print("Could not add logconfig since some variables are not in TOC")
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 _log_stab_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global g_roll, g_yaw, g_pitch, g_thrust
(g_roll, g_yaw, g_pitch) = (data["stabilizer.roll"], data["stabilizer.yaw"], data["stabilizer.pitch"])
# def _log_acc_data(self, timestamp, data, logconf):
# """Callback froma the log API when data arrives"""
# global g_acc_x, g_acc_y, g_acc_z;
# (g_acc_x, g_acc_y, g_acc_z) = (data["acc.x"], data["acc.y"], data["acc.z"])
# def _log_gyro_data(self, timestamp, data, logconf):
# """Callback froma the log API when data arrives"""
# global g_gyro_x, g_gyro_y, g_gyro_z;
# (g_gyro_x, g_gyro_y, g_gyro_z) = (data["gyro.x"], data["gyro.y"], data["gyro.z"])
def _log_baro_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global g_baro;
g_baro = data["baro.aslLong"]
def _control(self):
global g_roll, g_yaw, g_pitch, g_thrust
global g_gyro_x, g_gyro_y, g_gyro_z
global g_acc_x, g_acc_y, g_acc_z
global g_baro, g_init_baro, g_kill
global diff_baro
global thrust, thrustInit, scaleUp, scaleDown, thrustMin, thrustMax
global count
print "Control Started"
while not g_kill:
#print "acc.x = %10f, acc.y = %10f, acc.z = %10f" % (g_acc_x, g_acc_y, g_acc_z),
#print "gyro.x = %10f, gyro.y = %10f, gyro.z = %10f" % (g_gyro_x, g_gyro_y, g_gyro_z),
#print "thrust: %d" % thrust,
print "difference: %10f" % diff_baro,
print "baro = %10f, init_baro = %10f" % (g_baro, g_init_baro),
print "roll = %10f, yaw = %10f, pitch = %10f" % (g_roll, g_yaw, g_pitch)
if not (g_init_baro == 0):
diff_baro = g_init_baro - g_baro
roll = 0
pitch = 0
yawrate = 0
count = count + 1
if diff_baro > 0.0:
thrust = thrustInit + scaleUp * diff_baro
else:
thrust = thrustInit + scaleDown * diff_baro
if thrust < thrustMin:
thrust = thrustMin
if thrust > thrustMax:
thrust = thrustMax
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.01)
# thrust_mult = 1
# thrust_step = 500
# thrust = 20000
# pitch = 0
# roll = 0
# yawrate = 0
# while thrust >= 20000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# if thrust >= 25000:
# thrust_mult = -1
# thrust += thrust_step * thrust_mult
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
self._cf.close_link()
def _input(self):
global g_baro, g_init_baro, g_kill
print "Input Started"
getchVar = _Getch()
while g_kill is False:
if (getchVar == 's'):
g_init_baro = g_baro-1
elif (getchVar == 'k'):
self._cf.commander.send_setpoint(0,0,0,0)
g_kill = True
print "KILLED!!!"
#while g_kill is False:
# command = raw_input("Input Command:\n")
# if (command[0] == 'S') or (command[0] == 's'):
# g_init_baro = g_baro-1
# elif (command[0] == 'K') or (command[0] == 'k'):
# self._cf.commander.send_setpoint(0, 0, 0, 0)
# g_kill = True
# print "KILLED!!!"
#def setup_term(fd, when=termios.TCSAFLUSH):
# mode = termios.tcgetattr(fd)
# mode[tty.LFLAG] = mode[tty.LFLAG] & ~(termios.ECHO | termios.ICANON)
# termios.tcsetattr(fd, when, mode)
#def getch(timeout=None):
# fd = sys.stdin.fileno()
# old_settings = termios.tcgetattr(fd)
# try:
# setup_term(fd)
# try:
# rw, wl, xl = select.select([fd], [], [], timeout)
# except select.error:
# return
# if rw:
# return sys.stdin.read(1)
# finally:
# termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
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 Quad:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print "Connecting to %s" % link_uri
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print "Connected to %s" % link_uri
Thread(target=self._control).start()
########################################################################
# Set up Stabilizer Logger
########################################################################
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=10)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._log_stab_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since some variables are not in TOC")
########################################################################
# Set up Acc Logger
########################################################################
self._lg_acc = LogConfig(name="Acc", period_in_ms=10)
self._lg_acc.add_variable("acc.x", "float")
self._lg_acc.add_variable("acc.y", "float")
self._lg_acc.add_variable("acc.z", "float")
self._cf.log.add_config(self._lg_acc)
if self._lg_acc.valid:
# This callback will receive the data
self._lg_acc.data_received_cb.add_callback(self._log_acc_data)
# This callback will be called on errors
self._lg_acc.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_acc.start()
else:
print("Could not add logconfig since some variables are not in TOC")
########################################################################
# Set up Gyro Logger
########################################################################
self._lg_gyro = LogConfig(name="Gyro", period_in_ms=10)
self._lg_gyro.add_variable("gyro.x", "float")
self._lg_gyro.add_variable("gyro.y", "float")
self._lg_gyro.add_variable("gyro.z", "float")
self._cf.log.add_config(self._lg_gyro)
if self._lg_gyro.valid:
# This callback will receive the data
self._lg_gyro.data_received_cb.add_callback(self._log_gyro_data)
# This callback will be called on errors
self._lg_gyro.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_gyro.start()
else:
print("Could not add logconfig since some variables are not in TOC")
def _control(self):
global g_roll, g_yaw, g_pitch;
global g_acc_x, g_acc_y, g_acc_z;
while True:
print "Acc : x = %10f, y = %10f, z = %10f" % (g_acc_x, g_acc_y, g_acc_z)
print "Gyro : x = %10f, y = %10f, z = %10f" % (g_gyro_x, g_gyro_y, g_gyro_z)
# print "Stab : roll = %10f, yaw = %10f, pitch = %10f" % (g_roll, g_yaw, g_pitch)
time.sleep(0.1)
# thrust_mult = 1
# thrust_step = 500
# thrust = 20000
# pitch = 0
# roll = 0
# yawrate = 0
# while thrust >= 20000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# if thrust >= 25000:
# thrust_mult = -1
# thrust += thrust_step * thrust_mult
# self._cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
# time.sleep(0.1)
# self._cf.close_link()
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 _log_stab_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global g_roll, g_yaw, g_pitch;
(g_roll, g_yaw, g_pitch) = (data["stabilizer.roll"], data["stabilizer.yaw"], data["stabilizer.pitch"])
def _log_acc_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global g_acc_x, g_acc_y, g_acc_z;
(g_acc_x, g_acc_y, g_acc_z) = (data["acc.x"], data["acc.y"], data["acc.z"])
def _log_gyro_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global g_gyro_x, g_gyro_y, g_gyro_z;
(g_gyro_x, g_gyro_y, g_gyro_z) = (data["gyro.x"], data["gyro.y"], data["gyro.z"])
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:
# Initial values, you can use these to set trim etc.
roll = 0.0
pitch = 0.0
yawrate = 0
thrust = 10001
# desire asl value, it could be changed current_asl_value + 50
d_asl = 685
def __init__(self):
self.crazyflie = Crazyflie()
cflib.crtp.init_drivers(enable_debug_driver=False)
print "Scanning interfaces for Crazyflies..."
available = cflib.crtp.scan_interfaces()
print "Crazyflies found: "
for i in available:
print i[0]
# You may need to update this value if your Crazyradio uses a different frequency.
link_uri = available[0][0]
#link_uri = available[0][0]
#self.crazyflie.open_link(link_uri)
self.crazyflie.open_link("radio://0/80/250K")
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):
# Keep the commands alive so the firmware kill-switch doesn't kick in.
Thread(target=self.pulse_command).start()
hold_flag = True
# The definition of the logconfig can be made before connecting
self._lg_alt = LogConfig(name="altitude", period_in_ms=10)
self._lg_alt.add_variable("baro.asl", "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.
self._cf.log.add_config(self._lg_alt)
if self._lg_alt.valid:
# This callback will receive the data
self._lg_alt.data_received_cb.add_callback(self._alt_log_data)
# This callback will be called on errors
self._lg_alt.error_cb.add_callback(self._alt_log_error)
# Start the logging
self._lg_alt.start()
else:
print("Could not add logconfig since some variables are not in TOC")
while 1:
#self.crazyflie.log.
self.thrust = int(raw_input("Set thrust (10001-60000):"))
if self.thrust == 0:
self.crazyflie.close_link()
break
elif self.thrust <= 10000:
self.thrust = 10001
elif self.thrust == 40000:
# Test altitude hold flightmode
time.sleep(10)
#self.crazyflie.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.05)
self.param.set_value("flightmode.althold", "True")
#time.sleep(10)
#self.param.set_value("flightmode.althold", "False")
elif self.thrust > 60000:
self.thrust = 60000
def _alt_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print "Error when logging %s: %s" % (logconf.name, msg)
def _alt_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
if logconf.name == "altitude":
if not self._takeoff:
self._start_alt = data['baro.asl']
self._takeoff = True
else:
self._alt = data['baro.asl']
print "{}".format(self._alt - self._start_alt)
def pulse_command(self):
self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yawrate, self.thrust)
time.sleep(0.1)
# ..and go again!
self.pulse_command()
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 TestFlight:
def __init__(self):
"""
Initialize the quadcopter
"""
self.crazyflie = cflib.crazyflie.Crazyflie()
logging.info("Initializing drivers")
cflib.crtp.init_drivers()
logging.info("Searching for available devices")
available = cflib.crtp.scan_interfaces()
radio = False
for i in available:
# Connect to the first device of the type 'radio'
if 'radio' in i[0]:
radio = True
dev = i[0]
logging.info("Connecting to interface with URI [{0}] and name {1}".format(i[0], i[1]))
self.crazyflie.open_link(dev)
break
if not radio:
logging.info("No quadcopter detected. Try to connect again.")
exit(-1)
# Set up the callback when connected
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
"""
Set the loggers
"""
# Log stabilizer
self.logStab = LogConfig("Stabalizer", 200)
self.logStab.add_variable("stabilizer.roll", "float")
self.logStab.add_variable("stabilizer.pitch", "float")
self.logStab.add_variable("stabilizer.yaw", "float")
self.logStab.add_variable("stabilizer.thrust", "uint16_t")
self.crazyflie.log.add_config(self.logStab)
if self.logStab.valid:
self.logStab.data_received_cb.add_callback(self.print_stab_data)
self.logStab.start()
else:
logger.warning("Could not setup log configuration for stabilizer after connection!")
# Log barometer
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable("baro.aslLong", "float")
self.crazyflie.log.add_config(self.logBaro)
if self.logBaro.valid:
self.logBaro.data_received_cb.add_callback(self.print_baro_data)
self.logBaro.start()
else:
logger.warning("Could not setup log configuration for barometer after connection!")
# Log Accelerometer
self.logAccel = LogConfig("Accel",200)
self.logAccel.add_variable("acc.x", "float")
self.logAccel.add_variable("acc.y", "float")
self.logAccel.add_variable("acc.z", "float")
self.crazyflie.log.add_config(self.logAccel)
if self.logAccel.valid:
self.logAccel.data_received_cb.add_callback(self.print_accel_data)
self.logAccel.start()
else:
logger.warning("Could not setup log configuration for accelerometer after connection!")
# Call the requested thrust profile.
# Start a separate thread to run test.
# Do not hijack the calling thread!
if args.thrust_profile == 'increasing_step':
Thread(target=self.increasing_step).start()
if args.thrust_profile == 'prbs_hover':
Thread(target=self.prbs_hover).start()
if args.thrust_profile == 'prbs_asc':
Thread(target=self.prbs_asc).start()
if args.thrust_profile == 'prbs_desc':
Thread(target=self.prbs_desc).start()
def print_baro_data(self, ident, data, logconfig):
logging.info("Id={0}, Barometer: asl={1:.4f}".format(ident, data["baro.aslLong"]))
def print_stab_data(self, ident, data, logconfig):
logging.info("Id={0}, Stabilizer: Roll={1:.2f}, Pitch={2:.2f}, Yaw={3:.2f}, Thrust={4:.2f}".format(ident, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"]))
def print_accel_data(self, ident, data, logconfig):
logging.info("Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}".format(ident, data["acc.x"], data["acc.y"], data["acc.z"]))
# Thrust Profiles
def increasing_step(self):
thrust = 30000
pitch = 0
roll = 0
yaw_rate = 0
self.crazyflie.commander.send_setpoint(roll, pitch, yaw_rate, thrust)
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()
def prbs_hover(self):
pass
def prbs_asc(self):
pass
def prbs_desc(self):
pass
def get_accelerometer_log_config(self):
log_conf = LogConfig("Accel", period_in_ms=100)
log_conf.add_variable("acc.x", "float")
log_conf.add_variable("acc.y", "float")
log_conf.add_variable("acc.z", "float")
return log_conf
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 HoverTest:
"""Example that connects to the first Crazyflie found, hovers, and 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)
self._status = {}
self._status['stabilizer.roll'] = 0
self._status['stabilizer.thrust'] = 0
self._status['stabilizer.yaw'] = 0
self._status['stabilizer.pitch'] = 0
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)
# The definition of the logconfig can be made before connecting
self._lg_stabilizer = LogConfig(name="stabilizer", period_in_ms=10)
self._lg_stabilizer.add_variable("stabilizer.roll", "float")
self._lg_stabilizer.add_variable("stabilizer.thrust", "uint16_t")
self._lg_stabilizer.add_variable("stabilizer.yaw", "float")
self._lg_stabilizer.add_variable("stabilizer.pitch", "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_stabilizer)
# This callback will receive the data
self._lg_stabilizer.data_received_cb.add_callback(self._stabilizer_log_data)
# This callback will be called on errors
self._lg_stabilizer.error_cb.add_callback(self._stabilizer_log_error)
# Start the logging
self._lg_stabilizer.start()
except KeyError as e:
print("Could not start log configuration,"
"{} not found in TOC".format(str(e)))
except AttributeError:
print("Could not add stabilizer log config, bad configuration.")
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._hover).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 _stabilizer_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print("Error when logging %s: %s" % (logconf.name, msg))
def _stabilizer_log_data(self, timestamp, data, logconf):
#print("[%d][%s]: %s" % (timestamp, logconf.name, data))
self._status['stabilizer.roll'] = data['stabilizer.roll']
self._status['stabilizer.thrust'] = data['stabilizer.thrust']
self._status['stabilizer.yaw'] = data['stabilizer.yaw']
self._status['stabilizer.pitch'] = data['stabilizer.pitch']
def _hover(self):
global mpitch, mthrust, myaw, mroll, run
# Unlock startup thrust protection.
#self._cf.commander.send_setpoint(0, 0, 0, 0)
# Turn on altitude hold.
#self._cf.param.set_value("flightmode.althold", "True")
#f = open("Log.txt", "w")
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.5)
print "Unlocked"
while run:
# Update the position.
#print("Setting Point: ")
if ((mthrust >= minthrust)&(mgrab == 0)):
self._cf.commander.send_setpoint(mroll, mpitch, myaw, mthrust)
time.sleep(0.01)
else:
self._cf.commander.send_setpoint(0, 0, 0, 0) #turns off wothout delay and initially unlocks the thrust
print "zero"
print " pitch: %6.2f %6.2f, roll: %6.2f %6.2f, yaw: %6.2f %6.2f, mthrust: %6.2f %6.2f, grab: %2f" % (
mpitch,
self._status['stabilizer.pitch'],
mroll,
self._status['stabilizer.roll'],
myaw,
self._status['stabilizer.yaw'],
mthrust,
self._status['stabilizer.thrust'],
mgrab)
#f.write("%f %f %f %f\n" % (self._status['stabilizer.roll'], self._status['stabilizer.pitch'], mroll, mpitch))
#self._cf.commander.send_setpoint(0, 0, 0, 0)
#f.close()
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
print "run %f" % run
time.sleep(0.1)
self._cf.close_link()
class HoverExample:
def __init__(self, link_uri):
""" global loop vars """
self._exit = False
""" camera setup """
cam_ip = raw_input("Specify camera; enter for webcam, or ip of network :\n")
if cam_ip is '':
self._cam = SimpleCV.Camera()
elif '.' not in cam_ip:
self._cam = SimpleCV.JpegStreamCamera("http://192.168.1." + cam_ip + ":8080/video")
else:
self._cam = SimpleCV.JpegStreamCamera("http://" + cam_ip + ":8080/video")
self._img_size = (800,600)
self._img_set_point_accepted = False
self._img_set_point = 0
self._img_color = SimpleCV.Color.RED
self._img_blob_min = 10
self._img_blob_max = 1000
self._img_log_path = '/tmp/hover.img.log'
# start the vision loop -- will need to initialize though
# Thread(target=self._vision_loop).start()
#raw_input("Place drone at set point; then press enter:\n")
self._img_set_point_accepted = True
#self._img_set_point = self._img_pos.y
self._img_set_point = 480
raw_input("Set-point is {0}; press any key to conintue.\n".format(self._img_set_point))
""" 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._hover_target = 1
self._flight_time = 10
self._flight_log_path = "/tmp/hover.log"
self._asl = 0
self._alt_accepted = False
print "Connecting to %s" % link_uri
self._cf.open_link(link_uri)
# raw_input("Acquiring altitude reading. Press any key to accept\n")
self._alt_accepted = True
raw_input("Beginning hover. Press any key to stop\n")
self._exit = 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
self._log = LogConfig(name="Stabilizer", period_in_ms=10)
self._log.add_variable("baro.aslRaw", "float")
self._cf.log.add_config(self._log)
if self._log.valid:
self._log.data_received_cb.add_callback(self._log_data)
self._log.error_cb.add_callback(self._log_error)
self._log.start()
else:
print("Could not add logconfig -- TOC?")
Thread(target=self._hover_loop).start()
def _log_data(self, timestamp, data, logconf):
self._asl = data["baro.aslRaw"]
# print "[%d][%s]: %s" % (timestamp, logconf.name, self._asl)
def _log_error(self, logconf, msg):
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)
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 _vision_loop(self):
while not self._exit:
self._vision_cycle(show=True)
def _vision_cycle(self, show=False):
# log = open(self._img_log_path, 'w', 0)
# while not self._exit:
img = self._cam.getImage()
# image adjustments
if self._img_size:
img = img.resize(self._img_size[0], self._img_size[1])
img = img.rotate90()
# blob search
color = img - img.colorDistance(self._img_color)
blobs = color.findBlobs(-1, self._img_blob_min, self._img_blob_max)
# blob find
if blobs is not None:
self._img_pos = blobs[-1]
print blobs[-1]
# blob show
if show:
if blobs is not None:
roiLayer = SimpleCV.DrawingLayer((img.width, img.height))
for blob in blobs:
blob.draw(layer=roiLayer)
roiLayer.circle((self._img_pos.x,self._img_pos.y), 50, SimpleCV.Color.RED, 2)
img.addDrawingLayer(roiLayer)
blobs.draw(SimpleCV.Color.GREEN, 1)
img = img.applyLayers()
img.show()
# dy is the desired recalibration
return self._img_pos.y - self._img_set_point
# log.write("{y},{dy}\n".format(y=self._img_pos.y,dy=dy))
#log.close()
def _hover_loop(self):
"""
# aquire asl_start over first 3 seconds -- 30 samples
asl_start = 0
asl_readings = 0
while not self._alt_accepted or asl_readings == 0:
time.sleep(0.01)
asl_start += self._asl
asl_readings += 1
print "\rAltitude reading = %s (#%d, last=%s)" % (asl_start / asl_readings, asl_readings, self._asl),
asl_start = asl_start / asl_readings
# print out
print ""
print ""
print "Starting altitude = %s" % asl_start
asl_target = asl_start + self._hover_target
print "Hover target = %s" % self._hover_target
print "Target altitude = %s" % asl_target
print "Flight time = %s" % self._flight_time
"""
# flight loop
flight_time_start = time.time()
control_time_last = 0
hover_thrust = 38000
corrective_thrust = 5000
dy_scale = 1.0/400.0 # 800 pixels tall
while time.time() - flight_time_start < 60 and not self._exit:
if time.time() - control_time_last > 0.1: # control at most every 10ms
control_time_last = time.time()
dy = self._vision_cycle()
thrust = hover_thrust + (dy * dy_scale * corrective_thrust)
print "dy=%s, thrust=%s" % (dy, thrust)
# self._cf.commander.send_setpoint(0,0,0,thrust)
"""
asl_diff = asl_target - self._asl
# lift off speed
if asl_diff > 0.6:
thrust = 42000
# come down speed
elif asl_diff < -25:
thrust = 25000
# get to hover point
else:
thrust = hover_thrust + asl_diff * corrective_thrust
# update our concept of what hover thrust is
print "%s : %s (last=%s)" % (asl_diff, thrust, self._asl)
self._cf.commander.send_setpoint(0,0,0,thrust)
# upwards corrections seem fine
# drift down seems worse
#
"""
print "Killing Thrust"
self._cf.commander.send_setpoint(0,0,0,0)
time.sleep(1)
print "Stopped."
self._cf.close_link()
'''
class Crazy:
def __init__(self,ui):
# zmienne pomocnicze w obsludze GUI
self.ui=ui
self.thrust=0
self.pitch=0
self.roll=0
self.yaw=0
self.startup=True
self.is_connected=False
# ustawienia biblioteki cf
self.cf=Crazyflie()
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
#funkcja inicjujaca polaczenie
def connect(self,uri):
self.cf.open_link(uri)
self.is_connected=True
# funkcja wywolywana w chwili odebrania informacji o podlaczeniu sie
def connected(self,uri):
self.ui.l_conn.setText("Connected to {}".format(uri))
print("Connected to {}".format(uri))
control=Thread(target=self.send_ctrl)
control.start()
# self.log_thread()
# funkcja wywolywana w chwili zakonczenia transmisji
def disconnected(self,uri):
self.ui.l_conn.setText("Disconnected")
print("disconnected from {}".format(uri))
self.is_connected=False
# funkcja konczaca polaczenie
def close(self):
self.cf.close_link()
# ustawienia watku zbierajacego dane
def log_thread(self):
self.log = LogConfig(name="logs", period_in_ms=100)
self.log.add_variable("stabilizer.roll", "float")
self.log.add_variable("stabilizer.pitch", "float")
self.log.add_variable("stabilizer.yaw", "float")
try:
self.cf.log.add_config(self.log)
# This callback will receive the data
self.log.data_received_cb.add_callback(self.log_received)
# # This callback will be called on errors
self.log.error_cb.add_callback(self.log_error)
# Start the logging
self.log.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."
# odbieranie danych
def log_received(self,timestamp, data, logconf):
self.katy[0]=data["stabilizer.roll"]
self.katy[1]=data["stabilizer.pitch"]
self.katy[2]=data["stabilizer.yaw"]
self.ui.l_pitch_a.setText("Pitch: {:.3f}".format(self.katy[1]))
self.ui.l_roll_a.setText("Roll: {:.3f}".format(self.katy[0]))
self.ui.l_yaw_a.setText("Yaw: {:.3f}".format(self.katy[2]))
# print("{}, {} = {}".format(timestamp,logconf.name,data["acc.x"]))
def log_error(self,logconf, msg):
print("error while loggoing {}\n{}".format(logconf.name,msg))
# zmiana ustawien sterowania
def update_ctrl(self,thrust,pitch,roll,yaw):
print("thrust changed to {}".format(thrust))
print("yaw changed to {}".format(yaw))
print("pitch changed to {}".format(pitch))
print("roll changed to {}".format(roll))
self.thrust=thrust
self.pitch=pitch
self.roll=roll
self.yaw=yaw
# watek wysylajacy sterowanie
def send_ctrl(self):
while True:
if self.thrust > 60000:
self.thrust=60000
if self.thrust < 0:
self.thrust=0
#Unlock startup thrust protection
if self.startup:
self.cf.commander.send_setpoint(0,0,0,0)
self.startup=False
self.cf.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust)
sleep(0.01)
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=1)
#self._pubTemp = rospy.Publisher('temperature', Temperature, queue_size=1)
#self._pubMag = rospy.Publisher('magnetic_field', MagneticField, queue_size=1)
#self._pubPressure = rospy.Publisher('pressure', Float32, queue_size=1)
self._pubBattery = rospy.Publisher('battery', Float32, queue_size=1)
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._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
#print('pub battery')
#print(rospy.get_rostime())
msg.data = data["pm.vbat"]
self._pubBattery.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 = min(max(10000, int(self._cmdVel.linear.z)),60000)
#print(roll, pitch, yawrate, thrust)
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
#self._cf.commander.send_setpoint(roll, pitch, yawrate, 10000)
def _cmdVelChanged(self, data):
self._cmdVel = data
self._send_setpoint()
def _update(self):
#r = rospy.Rate(100)
while not rospy.is_shutdown():
#sec =rospy.get_time()
if self._state == CrazyflieROS.Disconnected:
self._try_to_connect()
#rospy.sleep(0.2)
#print(0)
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()
#print(1)
else:
self._cmdVel = Twist()
self._send_setpoint()
#print(2)
rospy.sleep(0.2)
#rospy.sleep(0.2)
else:
#While is connecting, wait
#rospy.sleep(0.2)
rospy.sleep(0.5)
#sec = 1
#rospy.sleep(0.03)
#r.sleep()
#rospy.spin()
#print(rospy.get_time()-sec)
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)
#print(4)
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."""
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=10)
#self._lg_stab.add_variable("stabilizer.roll", "float")
#self._lg_stab.add_variable("stabilizer.pitch", "float")
#self._lg_stab.add_variable("stabilizer.yaw", "float")
self._lg_stab.add_variable("stabilizer.thrust", "float")
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since some variables are not in TOC")
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._ramp_motors).start()
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print "Error when logging %s: %s" % (logconf.name, msg)
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print "[%d][%s]: %s" % (timestamp, logconf.name, data)
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print "Connection to %s failed: %s" % (link_uri, msg)
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print "Connection to %s lost: %s" % (link_uri, msg)
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print "Disconnected from %s" % link_uri
def _ramp_motors(self):
thrust = 45000
thrust_step = 250
self._cf.param.set_value("flightmode.althold", "False")
self._cf.commander.send_setpoint(0,0,0,0)
totalTime = 0
self._cf.commander.send_setpoint(0, 0, 0, 60000)
time.sleep (.5)
self._cf.commander.send_setpoint(0, 0, 0, 60000)
self._cf.param.set_value("flightmode.althold", "True")
#time.sleep (.5)
while totalTime <= 1:
self._cf.commander.send_setpoint(0, -1, 0, 44500)
time.sleep (.1)
totalTime += .01
#self._cf.param.set_value("flightmode.althold", "False")
time.sleep (.5)
while thrust >=5000:
thrust -= thrust_step
self._cf.commander.send_setpoint(0, 0, 0, thrust)
time.sleep (.01)
self._cf.commander.send_setpoint(0, 0, 0, 0)
print "done"
self._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 """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print("Connecting to %s" % link_uri)
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print("Connected to %s" % link_uri)
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=10)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("acc.zw", "float")
self._lg_stab.add_variable("rangefinder.range")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.log.add_config(self._lg_stab)
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
except KeyError as e:
print("Could not start log configuration,"
"{} not found in TOC".format(str(e)))
except AttributeError:
print("Could not add Stabilizer log config, bad configuration.")
# Start a timer to disconnect in 10s
t = Timer(5, self._cf.close_link)
t.start()
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print("Error when logging %s: %s" % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print("[%d][%s]: %s" % (timestamp, logconf.name, data))
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print("Connection to %s failed: %s" % (link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print("Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print("Disconnected from %s" % link_uri)
self.is_connected = False
def get_stabilizer_log_config(self):
stab_log_conf = LogConfig("stabilizer", period_in_ms=100)
stab_log_conf.add_variable("stabilizer.roll", "float")
stab_log_conf.add_variable("stabilizer.pitch", "float")
stab_log_conf.add_variable("stabilizer.yaw", "float")
return stab_log_conf
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()
def get_gyroscope_log_config(self):
log_conf = LogConfig("GyroScope", period_in_ms=100)
log_conf.add_variable("gyro.x", "float")
log_conf.add_variable("gyro.y", "float")
log_conf.add_variable("gyro.z", "float")
return log_conf
class Logging:
#
#Logging class that logs the Stabilizer from a supplied
#link uri.
#
def __init__(self, link_uri):
#Initialize and run the example with the specified link_uri
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print "Connecting to %s" % link_uri
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
#This callback is called form the Crazyflie API when a Crazyflie
#has been connected and the TOCs have been downloaded.
print "Connected to %s" % link_uri
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=10)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._lg_stab.add_variable("acc.x", "float")
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since stab variables are not in TOC")
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):
global data_stabilizer
data_stabilizer = (data["stabilizer.roll"], data["stabilizer.pitch"],data["stabilizer.yaw"])
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 Crazy:
def __init__(self, l_gyro, l_acc, l_status):
# zmienne pomocnicze w obsludze GUI
self.conn = l_status
self.l_gyro = l_gyro
self.l_acc = l_acc
self.is_connected = False
self.r = 0
self.p = 0
self.y = 0
# ustawienia biblioteki cf
self.cf = Crazyflie()
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
#funkcja inicjujaca polaczenie
def connect(self, uri):
self.cf.open_link(uri)
self.is_connected = True
# funkcja wywolywana w chwili odebrania informacji o podlaczeniu sie
def connected(self, uri):
self.conn.setText("Connected to {}".format(uri))
print("Connected to {}".format(uri))
self.log_thread()
Thread(target=self.send_ctrl).start()
# funkcja wywolywana w chwili zakonczenia transmisji
def disconnected(self, uri):
self.conn.setText("Disconnected")
print("disconnected from {}".format(uri))
self.is_connected = False
# funkcja konczaca polaczenie
def close(self):
self.cf.close_link()
# ustawienia watku zbierajacego dane
def log_thread(self):
self.log_acc = LogConfig(name="acc", period_in_ms=100)
self.log_acc.add_variable("acc.x", "float")
self.log_acc.add_variable("acc.y", "float")
self.log_acc.add_variable("acc.z", "float")
self.log_acc.add_variable("gyro.x", "float")
self.log_acc.add_variable("gyro.y", "float")
self.log_acc.add_variable("gyro.z", "float")
try:
self.cf.log.add_config(self.log_acc)
# This callback will receive the data
self.log_acc.data_received_cb.add_callback(self.log_received_acc)
# # This callback will be called on errors
self.log_acc.error_cb.add_callback(self.log_error)
# Start the logging
self.log_acc.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."
# odbieranie danych
def log_received_acc(self, timestamp, data, logconf):
rate = 0.1
self.r += data['gyro.x'] * rate
self.p += data['gyro.y'] * rate
self.y += data['gyro.z'] * rate
self.l_acc[0].setText("roll: {:.3f}".format(self.r))
self.l_acc[1].setText("pitch: {:.3f}".format(self.p))
self.l_acc[2].setText("yaw: {:.3f}".format(self.y))
self.l_gyro[0].setText("x: {:.3f}".format(data["gyro.x"]))
self.l_gyro[1].setText("y: {:.3f}".format(data["gyro.y"]))
self.l_gyro[2].setText("z: {:.3f}".format(data["gyro.z"]))
# print("{}, {} = {}".format(timestamp,logconf.name,data["acc.x"]))
def log_error(self, logconf, msg):
print("error while loggoing {}\n{}".format(logconf.name, msg))
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri,results, size=(600,500)):
self.thrust_mult = 1
self.thrust_step = 500
self.thrust = 20000
self.pitch = 0
self.roll = 0
self.yawrate = 0
""" Initialize and run the example with the specified link_uri """
#self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
#self.s.setblocking(0)
#self.s.bind((TCP_IP, TCP_PORT))
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
print "Result test: ",results
# 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
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
self.app = QtGui.QApplication([])
self.interval = int(0.1*1000)
self.bufsize = 60
self.databuffer = collections.deque([0,0]*self.bufsize, self.bufsize)
self.x = [0.0 , 1.0]#linspace(0,1000,60)
self.y = zeros(self.bufsize, dtype=float)
self.plt = pg.plot(title = 'Real Time Sensor Evaluation')
self.plt.resize(*size)
self.plt.showGrid(x=True, y=True)
#self.plt.setYRange(-20,30, padding = 0)
self.curve = self.plt.plot(self.x,results,pen=(255,0,0))
self.timer = QtCore.QTimer()
self.timer.timeout.connect(self.updateplot)
self.timer.start(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
# The definition of the logconfig can be made before connecting
#changed
#self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=10)
#self._lg_stab.add_variable("stabilizer.roll", "float")
#self._lg_stab.add_variable("stabilizer.pitch", "float")
#self._lg_stab.add_variable("stabilizer.yaw", "float")
self._lg_stab = LogConfig(name="mb", period_in_ms=10)
self._lg_stab.add_variable("mb.distance", "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.
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since some variables are not in TOC")
# Start a timer to disconnect in 10s
t = Timer(1000000, 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 serv_listen(self):
self.s.listen(1)
self.conn, self.addr = self.s.accept()
self.data_buff = sel
f.conn.recv(BUFFER_SIZE)
#print self.data_buff
return self.data_buff
def updateplot(self):
#self.databuffer.append(results)
#self.y[:] = self.databuffer
#self.y.append(data)
results_plot = resultspyqt[-500:]
x_plot = self.x[-500:]
self.plt.setYRange(-30,30, padding = 0)
self.curve.setData(x_plot,results_plot)
self.app.processEvents()
def getdata(data):
str1=data['mb.distance']
num1=float(str1)
num1=30-num1
return num1
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
#print strftime("%H:%M:%S ", gmtime())
str1=data['mb.distance']
num1=float(str1)
num1=30-num1
#self.updateplot(num1)
#print "test: ",num1
#self.databuffer.append(num1)
#self.y[:] = self.databuffer
#self.curve.setData(x,num1)
#self.app.processEvents()
results.append(num1)
resultspyqt.append(num1)
self.x = list(range(0,len(resultspyqt)))
print "[%d][%s]: %s" % (timestamp, logconf.name, data)
#if not data: break
data=self.serv_listen()
if data>0:
print "app: ",data
if(int(data)<100):#we are in thrust
print "thrust"
print self.roll, self.pitch, self.yawrate, self.thrust
self.thrust=int(data)*600
self._cf.commander.send_setpoint(self.roll, self.pitch, self.yawrate, self.thrust)
#time.sleep(0.1)
elif((int(data)>100)and(int(data)<200)):#we are in pitch
print roll, pitch, yawrate, thrust
pitch=(int(data))/5-30
self._cf.commander.send_setpoint(roll, (int(data))/5-30, yawrate, thrust)
#time.sleep(0.1)
elif(int(data)>200):#we are in roll
print "add roll: ",150-(int(data))*3/5
print roll, pitch, yawrate, thrust
roll=50-(int(data))/5
self._cf.commander.send_setpoint(50-(int(data))/5, pitch, yawrate, thrust)
#time.sleep(0.1)
if data == 'Hover':
print "app: ",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)
#print results
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
#print results
def run(self):
self.app.exec_()
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 = True
def connected(self, uri):
print("Connected to {}".format(uri))
# Thread(target=self.motor).start()
self.log_thread()
def disconnected(self, uri):
print("disconnected from {}".format(uri))
def close(self):
self.cf.close_link()
def log_thread(self):
self.log = LogConfig(name="acc", period_in_ms=100)
self.log.add_variable("acc.x", "float")
self.log.add_variable("acc.y", "float")
self.log.add_variable("acc.z", "float")
try:
self.cf.log.add_config(self.log)
# This callback will receive the data
self.log.data_received_cb.add_callback(self.log_received)
# # This callback will be called on errors
self.log.error_cb.add_callback(self.log_error)
# Start the logging
self.log.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."
t = Timer(5, self.close)
def log_received(self, timestamp, data, logconf):
# self.x.setText("x: {}".format["acc.x"])
# self.y.setText("y: {}".format["acc.y"])
# self.z.setText("z: {}".format["acc.z"])
print("{}, {} = {} on {}".format(timestamp, logconf.name,
data["acc.x"], self.uri))
def log_error(self, logconf, msg):
print("error while loggoing {}\n{}".format(logconf.name, msg))
def motor(self):
thrust_mult = 1
thrust_step = 50
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)
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 Crazy(QObject):
cf_connected = pyqtSignal(bool)
cf_logs = pyqtSignal(dict)
def __init__(self, parent):
super(Crazy, self).__init__()
# zmienne pomocnicze w obsludze GUI
self.thrust = 0
self.pitch = 0
self.roll = 0
self.yaw = 0
self.startup = True
self.is_connected = False
self.busy = False
self.control_started = False
# zmienna do wysylania logow
self.log_data = {
"roll": 0.0,
"pitch": 0.0,
"yaw": 0.0,
"thrust": 0.0,
"m1": 0.0,
"m2": 0.0,
"m3": 0.0,
"m4": 0.0
}
# ustawienia dobierania danych do sterowania
parent.control_data_sig.connect(self.update_ctrl_sig)
# ustawienia biblioteki cf
self.cf = Crazyflie()
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
self.cf.connection_lost.add_callback(self.lost_connection)
#ustawienie watku sterowania
self.control = Thread(target=self.send_ctrl)
#funkcja inicjujaca polaczenie
def connect(self, uri):
self.cf.open_link(uri)
# funkcja wywolywana w chwili odebrania informacji o podlaczeniu sie
def connected(self, uri):
self.is_connected = True
print("Connected to {}".format(uri))
self.log_thread()
self.cf_connected.emit(self.is_connected)
self.control.start()
self.control_started = True
# funkcja wywolywana w chwili zakonczenia transmisji
def disconnected(self, uri):
print("disconnected from {}".format(uri))
self.is_connected = False
self.cf_connected.emit(self.is_connected)
if self.control_started:
self.control.join(0.1)
# funkcja wywolywana w chwili przerwania transmisji
def lost_connection(self, uri, var):
print("disconnected from {}".format(uri))
self.is_connected = False
self.cf_connected.emit(self.is_connected)
if self.control_started:
self.control.join(0.1)
# funkcja konczaca polaczenie
def close(self):
self.cf.close_link()
# ustawienia watku zbierajacego dane
def log_thread(self):
print "test"
self.log = LogConfig(name="logs", period_in_ms=10)
self.log.add_variable("stabilizer.roll", "float")
self.log.add_variable("stabilizer.pitch", "float")
self.log.add_variable("stabilizer.yaw", "float")
# self.log.add_variable("stabilizer.thrust", "float")
# self.log.add_variable("motor.m1", "float")
# self.log.add_variable("motor.m2", "float")
# self.log.add_variable("motor.m3", "float")
# self.log.add_variable("motor.m4", "float")
try:
print "test 2"
self.cf.log.add_config(self.log)
# This callback will receive the data
self.log.data_received_cb.add_callback(self.log_received)
# # This callback will be called on errors
self.log.error_cb.add_callback(self.log_error)
# Start the logging
self.log.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."
# odbieranie danych
def log_received(self, timestamp, data, logconf):
# print"log"
try:
self.log_data["roll"] = data["stabilizer.roll"]
self.log_data["pitch"] = data["stabilizer.pitch"]
self.log_data["yaw"] = data["stabilizer.yaw"]
# self.log_data["thrust"]=data["stabilizer.thrust"]
# self.log_data["m1"]=data["motor.m1"]
# self.log_data["m2"]=data["motor.m2"]
# self.log_data["m3"]=data["motor.m3"]
# self.log_data["m4"]=data["motor.m4"]
except:
print("blad logowania")
self.cf_logs.emit(self.log_data)
# print (self.log_data)
def log_error(self, logconf, msg):
("error while loggoing {}\n{}".format(logconf.name, msg))
# zmiana ustawien sterowania - stara wersja - nieuzywana
def update_ctrl(self, thrust, pitch, roll, yaw):
# ("thrust | pitch | roll | yaw ")
self.busy = True
# ("{:.3f} | {:.3f} | {:.3f} | {:.3f}".format(thrust,pitch,roll,yaw))
self.thrust = thrust
self.pitch = pitch
self.roll = roll
self.yaw = yaw
self.busy = False
# zmiana ustawien sterowania - aktualna wersja
def update_ctrl_sig(self, data):
# self.busy=True
# print("{:.3f} | {:.3f} | {:.3f} | {:.3f}".format(data["thrust"],data["pitch"],data["roll"],data["yaw"]))
self.thrust = data["thrust"]
self.pitch = data["pitch"]
self.roll = data["roll"]
self.yaw = data["yaw"]
# self.busy=False
# watek wysylajacy sterowanie
def send_ctrl(self):
while self.is_connected:
if not self.busy:
if self.thrust > 60000:
self.thrust = 60000
if self.thrust < 0:
self.thrust = 0
#Unlock startup thrust protection
if self.startup:
self.cf.commander.send_setpoint(0, 0, 0, 0)
self.startup = False
# self.cf.commander.set_client_xmode(False)
self.cf.commander.send_setpoint(self.roll, self.pitch,
self.yaw, self.thrust)
sleep(0.01)
class Listener(libmyo.DeviceListener):
interval = 0.05 # Output only 0.05 seconds
def __init__(self):
super(Listener, self).__init__()
self.orientation = None
self.pose = libmyo.Pose.rest
self.emg_enabled = False
self.locked = False
self.rssi = None
self.emg = None
self.last_time = 0
global channel
link_uri = "radio://0/" + channel + "/2M"
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.roll_p = 0
self.pitch_p = 0
self.roll_h = 0
self.pitch_h = 0
self.thrust_h = 0
self.holder = 0
self.lock = 1
self.quat = libmyo.Quaternion(0, 0, 0, 1)
quat_w = 0
quat_x = 0
quat_y = 0
quat_z = 0
print("Connecting to %s" % link_uri)
#MyoBand
def on_connect(self, myo, timestamp, firmware_version):
myo.vibrate('short')
myo.vibrate('short')
myo.request_rssi()
myo.request_battery_level()
def on_rssi(self, myo, timestamp, rssi):
self.rssi = rssi
def on_pose(self, myo, timestamp, pose):
if pose == libmyo.Pose.double_tap:
self.holder = 0
print("tap")
elif pose == libmyo.Pose.fingers_spread:
self._cf.commander.send_setpoint(0, 0, 0, 0) #Unlocks thrust
self.lock = 0
print("finger")
elif pose == libmyo.Pose.fist:
self._cf.commander.send_setpoint(0, 0, 0, 0) #Clear packets
time.sleep(0.1)
self.lock = 1
self._cf.close_link()
print("fist")
self.pose = pose
def on_orientation_data(self, myo, timestamp, quat):
roll_w = int(
math.atan2(2.0 * (quat.w * quat.x + quat.y * quat.z), 1.0 - 2.0 *
(quat.x * quat.x + quat.y * quat.y)) * 100 / math.pi)
thrust = float(
math.asin(
max(-1.0, min(1.0,
2.0 * (quat.w * quat.y - quat.z * quat.x)))))
if self.holder == 0:
self.thrust_h = int((thrust + math.pi / 2.0) / math.pi * 70000)
self.quat = libmyo.Quaternion(quat.x, quat.y, quat.z,
quat.w).conjugate()
self.holder = 1
tempquat = quat * self.quat
pitch_w = int(
math.atan2(
2.0 * (tempquat.w * tempquat.z + tempquat.x * tempquat.y),
1.0 - 2.0 *
(tempquat.y * tempquat.y + tempquat.z * tempquat.z)) * 100 /
math.pi)
thrust_w = abs(
int((thrust + math.pi / 2.0) / math.pi * 70000) - self.thrust_h)
print(pitch_w, " ", roll_w, " ", thrust_w)
yaw = 0
self._cf.commander.send_setpoint(roll_w, pitch_w, yaw, thrust_w)
self.orientation = quat
def on_accelerometor_data(self, myo, timestamp, acceleration):
pass
def on_gyroscope_data(self, myo, timestamp, gyroscope):
pass
def on_emg_data(self, myo, timestamp, emg):
self.emg = emg
def on_unlock(self, myo, timestamp):
self.locked = False
def on_lock(self, myo, timestamp):
self.locked = True
def on_event(self, kind, event):
pass
def on_event_finished(self, kind, event):
pass
def on_pair(self, myo, timestamp, firmware_version):
pass
def on_unpair(self, myo, timestamp):
pass
def on_disconnect(self, myo, timestamp):
pass
def on_arm_sync(self, myo, timestamp, arm, x_direction, rotation,
warmup_state):
pass
def on_arm_unsync(self, myo, timestamp):
pass
def on_battery_level_received(self, myo, timestamp, level):
pass
def on_warmup_completed(self, myo, timestamp, warmup_result):
pass
#CrazyFlie
def _connected(self, link_uri):
global connected
self._cf.commander.send_setpoint(0, 0, 0, 0)
connected = True
self._lg_acc = LogConfig(name="Acceleration", period_in_ms=10)
self._lg_acc.add_variable("acc.zw", "float")
self._lg_acc.add_variable("altHold.target", "float")
self._lg_acc.add_variable("stabilizer.thrust", "float")
try:
self._cf.log.add_config(self._lg_acc)
# This callback will receive the data
self._lg_acc.data_received_cb.add_callback(self._acc_log_data)
# This callback will be called on errors
self._lg_acc.error_cb.add_callback(self._acc_log_error)
# Start the logging
self._lg_acc.start()
except KeyError as e:
print("Could not start log configuration,"
"{} not found in TOC".format(str(e)))
except AttributeError:
print("Could not add Acceleration log config, bad configuration.")
def _connection_failed(self, link_uri, msg):
global connected
print("Connection to %s failed: %s" % (link_uri, msg))
connected = False
def _connection_lost(self, link_uri, msg):
global connected
print("Connection to %s lost: %s" % (link_uri, msg))
connected = False
def _disconnected(self, link_uri):
global connected
print("Disconnected from %s" % link_uri)
connected = False
def _acc_log_error(self, logconf, msg):
print("Error when logging %s: %s" % (logconf.name, msg))
def _acc_log_data(self, timestamp, data, logconf):
"""if(data["acc.zw"] < -0.98):
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie(ro_cache="./crazyflie-clients-python/lib/cflib/cache",
rw_cache="./crazyflie-clients-python/lib/cflib/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)
print "Connecting to %s" % link_uri
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print "Connected to %s" % link_uri
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=2000)
self._lg_stab.add_variable("gyro.x", "float")
self._lg_stab.add_variable("gyro.y", "float")
self._lg_stab.add_variable("gyro.z", "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")
self._lg_stab.add_variable("stabilizer.thrust", "uint16_t")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since some variables are not in TOC")
self._cf.commander.send_setpoint(0, 0, 0, 20000)
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)
new_dict = {logconf.name:data}
x = new_dict['Stabilizer']['stabilizer.roll']
y = new_dict['Stabilizer']['stabilizer.pitch']
z = new_dict['Stabilizer']['stabilizer.yaw']
print "%s" % y
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 Quad:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print "Connecting to %s" % link_uri
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print "Connected to %s" % link_uri
Thread(target=self._control).start()
Thread(target=self._input).start()
########################################################################
# Set up Stabilizer Logger
########################################################################
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=50)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._log_stab_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since some variables are not in TOC")
# ########################################################################
# # Set up Battery Logger
# ########################################################################
self._lg_bat = LogConfig(name="Battery",period_in_ms=50)
self._lg_bat.add_variable("pm.vbat", "float")
self._cf.log.add_config(self._lg_bat)
if self._lg_bat.valid:
# This callback will receive the data
self._lg_bat.data_received_cb.add_callback(self._log_bat_data)
# This callback will be called on errors
self._lg_bat.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_bat.start()
else:
print("Could not setup loggingblock! You dun goofed")
# ########################################################################
# # Set up Acc Logger
# ########################################################################
# self._lg_acc = LogConfig(name="Acc", period_in_ms=50)
# self._lg_acc.add_variable("acc.x", "float")
# self._lg_acc.add_variable("acc.y", "float")
# self._lg_acc.add_variable("acc.z", "float")
# self._cf.log.add_config(self._lg_acc)
# if self._lg_acc.valid:
# # This callback will receive the data
# self._lg_acc.data_received_cb.add_callback(self._log_acc_data)
# # This callback will be called on errors
# self._lg_acc.error_cb.add_callback(self._log_error)
# # Start the logging
# self._lg_acc.start()
# else:
# print("Could not add logconfig since some variables are not in TOC")
# ########################################################################
# # Set up Gyro Logger
# ########################################################################
# self._lg_gyro = LogConfig(name="Gyro", period_in_ms=50)
# self._lg_gyro.add_variable("gyro.x", "float")
# self._lg_gyro.add_variable("gyro.y", "float")
# self._lg_gyro.add_variable("gyro.z", "float")
# self._cf.log.add_config(self._lg_gyro)
# if self._lg_gyro.valid:
# # This callback will receive the data
# self._lg_gyro.data_received_cb.add_callback(self._log_gyro_data)
# # This callback will be called on errors
# self._lg_gyro.error_cb.add_callback(self._log_error)
# # Start the logging
# self._lg_gyro.start()
# else:
# print("Could not add logconfig since some variables are not in TOC")
# ########################################################################
# # Set up Baro Logger
# ########################################################################
self._lg_baro = LogConfig(name="Baro", period_in_ms=50)
self._lg_baro.add_variable("baro.aslLong", "float")
self._cf.log.add_config(self._lg_baro)
if self._lg_baro.valid:
# This callback will receive the data
self._lg_baro.data_received_cb.add_callback(self._log_baro_data)
# This callback will be called on errors
self._lg_baro.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_baro.start()
else:
print("Could not add logconfig since some variables are not in TOC")
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 _log_stab_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global g_roll, g_yaw, g_pitch, g_thrust
(g_roll, g_yaw, g_pitch) = (data["stabilizer.roll"], data["stabilizer.yaw"], data["stabilizer.pitch"])
# def _log_acc_data(self, timestamp, data, logconf):
# """Callback froma the log API when data arrives"""
# global g_acc_x, g_acc_y, g_acc_z;
# (g_acc_x, g_acc_y, g_acc_z) = (data["acc.x"], data["acc.y"], data["acc.z"])
# def _log_gyro_data(self, timestamp, data, logconf):
# """Callback froma the log API when data arrives"""
# global g_gyro_x, g_gyro_y, g_gyro_z;
# (g_gyro_x, g_gyro_y, g_gyro_z) = (data["gyro.x"], data["gyro.y"], data["gyro.z"])
def _log_baro_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global g_baro;
g_baro = data["baro.aslLong"]
def _log_bat_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global g_bat;
g_bat = data["pm.vbat"]
def _control(self):
global g_roll, g_yaw, g_pitch, g_thrust
global g_gyro_x, g_gyro_y, g_gyro_z
global g_acc_x, g_acc_y,g_acc_z
global g_baro, g_init_baro, g_kill
global diff_baro
global thrust, thrustInit, scaleUp, scaleDown, thrustMin, thrustMax
global count
global pause
global g_bat
print "Control Started"
while not g_kill:
if (count%100==0):
#print "acc.x = %10f, acc.y = %10f, acc.z = %10f" % (g_acc_x, g_acc_y, g_acc_z),
#print "gyro.x = %10f, gyro.y = %10f, gyro.z = %10f" % (g_gyro_x, g_gyro_y, g_gyro_z),
print "thrust: %d" % thrust,
print "battery: %10f" % g_bat,
print "baro = %10f, init_baro = %10f" % (g_baro, g_init_baro)
print "roll = %10f, yaw = %10f, pitch = %10f" % (g_roll, g_yaw, g_pitch)
count = count + 1
if not (g_init_baro == 0):
diff_baro = g_init_baro - g_baro
roll = 0
pitch = 0
yawrate = 0
if pause == 's':
thrust = thrustInit
elif pause == 'l':
thrust = 0
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.01)
# thrust_mult = 1
# thrust_step = 500
# thrust = 20000
# pitch = 0
# roll = 0
# yawrate = 0
# while thrust >= 20000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# if thrust >= 25000:
# thrust_mult = -1
# thrust += thrust_step * thrust_mult
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
self._cf.close_link()
def _input(self):
global g_baro, g_init_baro, g_kill, pause
print "Input Started"
while g_kill is False:
command = raw_input("Input Command:\n")
if (command[0] == 'S') or (command[0] == 's'):
g_init_baro = g_baro-1
pause = 's'
elif (command[0] == 'l'):
pause = 'l'
elif (command[0] == 'K') or (command[0] == 'k'):
self._cf.commander.send_setpoint(0, 0, 0, 0)
g_kill = True
print "KILLED!!!"
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 LogData:
def __init__(self, link_uri):
self.thrust = 10001
self.roll = self.pitch = self.yaw = 0
#Sensors being monitored
self.r = self.p = self.y = 0
self.ax = self.ay = self.az = 0
self.baro = 0
# Create a Crazyflie object without specifying any cache dirs
self.crazyflie = Crazyflie()
# The definition of the logconfig
self._lg_stab = LogConfig(name="Logging", period_in_ms=10)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._lg_stab.add_variable("acc.x", "float")
self._lg_stab.add_variable("acc.y", "float")
self._lg_stab.add_variable("acc.z", "float")
#self._lg_stab.add_variable("baro.aslLong", "float")
# Connect some callbacks from the Crazyflie API
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)
print("Connecting to %s" % link_uri)
# Try to connect to the Crazyflie
self.crazyflie.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = False
def _connected(self, link_uri):
print("Connected to %s" % link_uri)
self.crazyflie.is_connected = True
#create a thread that published the set points
#Thread(target=self.pulse_command).start()
# Adding the configuration
try:
#self.crazyflie.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()
print "yay"
Thread(target=self.pulse_command).start()
print "Hello I am out of "
except KeyError as e:
print("Could not start log configuration,"
"{} not found in TOC".format(str(e)))
except AttributeError:
print("Could not add Stabilizer log config, bad configuration.")
# Start a timer to disconnect in 10s
#t = Timer(5, self.crazyflie.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 _accel_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 from the log API when data arrives"""
self.r = data["stabilizer.roll"]
self.p = data["stabilizer.pitch"]
self.y = data["stabilizer.yaw"]
self.ax = data["acc.x"]
self.ay = data["acc.y"]
self.az = data["acc.z"]
#self.baro = data["baro.aslLong"]
print self.ax
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 pulse_command(self):
print 'in pulse_command'
self.crazyflie.commander.send_setpoint(self.roll, self.pitch, self.yaw, self.thrust)
#self.crazyflie.commander.send_setpoint(0,0,0,20000)
time.sleep(0.1)
class logs:
def __init__(self, cf):
#self.log_file_next_stuff = open("log_file_next.txt", "w+")
#self.log_file_stab = open("log_file_stab.txt", "w+")
#local copy of crazy_Auto
self._cf = cf
# Roll, Pitch, Yaw
self.init_state = [0,0,0]
self.state = [0,0,0]
self.next_state = [0,0]
# X, Y, Z, Mag
self.init_accel_state = [0,0,0,0]
self.accel_state = [0,0,0,0]
# X, Y, Z
self.init_gyro_state = [0,0,0]
self.gyro_state = [0,0,0]
# ground average, current
self.altitude = [0,0]
#current battery voltage
self.battery_state = 0
#[roll,pitch,yaw,altitude,acc.x,acc.y,acc.z,acc.mag,gyro.x, gyro.y, gyro.z, batteryV]
self.measurements = [0 for i in range(12)]
self.logging_count = 0
#Setup logs once connection to crazyflie has been established
self._cf._cf.connected.add_callback(self._init_flight_var)
# specifying the size of the state and the observation space
#self.transition_matrices = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
#self.transition_matrices.resize(4,1)
#self.kf = KalmanFilter(transition_matrices=self.transition_matrices, n_dim_obs= 4) #transition_matrices=self.transition_matrices,
#self._cf.connected.add_callback(self._init_flight_var) #fill in base parameters for flight
def _init_flight_var(self, link_uri):
print ("Connected to %s" % link_uri)
self.RPY_log = LogConfig(name="Stabilizer", period_in_ms=10)
self.RPY_log.add_variable("stabilizer.roll", "float")
self.RPY_log.add_variable("stabilizer.pitch", "float")
self.RPY_log.add_variable("stabilizer.yaw", "int16_t")
self.RPY_log.add_variable("baro.asl", "float") #barometer above sea level
self.RPY_log.add_variable("gyro.x", "float")
self.RPY_log.add_variable("gyro.y", "float")
self.RPY_log.add_variable("gyro.z", "float")
self.battery_log = LogConfig(name="Battery", period_in_ms=1000)
self.battery_log.add_variable("pm.vbat", "float")
self.acc_log = LogConfig(name="Acc", period_in_ms = 10)
self.acc_log.add_variable("acc.x", "float")
self.acc_log.add_variable("acc.y", "float")
self.acc_log.add_variable("acc.z", "float")
self._cf._cf.log.add_config(self.RPY_log) #add the log to the CrazyFlie
self._cf._cf.log.add_config(self.battery_log)
self._cf._cf.log.add_config(self.acc_log)
self.RPY_log.data_received_cb.add_callback(self.update_flight_params)
self.RPY_log.error_cb.add_callback(self.update_error)
self.battery_log.data_received_cb.add_callback(self.update_battery)
self.battery_log.error_cb.add_callback(self.update_error)
self.acc_log.data_received_cb.add_callback(self.update_acc)
self.acc_log.error_cb.add_callback(self.update_error)
self.RPY_log.start()
self.battery_log.start()
self.acc_log.start()
print ("Logging Started\n")
self._cf._cf.connected.add_callback(self._cf._connected)
def update_error(self, logconf, msg):
print ("Error when logging %s: %s" % (logconf.name, msg))
def update_battery(self, timestamp, data, logconf):
self.battery_state = data["pm.vbat"]
def update_acc(self, timestamp, data, logconf):
if not self._cf.is_flying:
self.init_accel_state[0] = data["acc.x"]
self.init_accel_state[1] = data["acc.y"]
self.init_accel_state[2] = data["acc.z"]
self.init_accel_state[3] = data["acc.mag2"]
return
self.accel_state[0] = data["acc.x"]
self.accel_state[1] = data["acc.y"]
self.accel_state[2] = data["acc.z"]
self.accel_state[3] = data["acc.mag2"]
def update_flight_params(self, timestamp, data, logconf):
#print data
#print self._cf.is_flying
if not self._cf.is_flying:
self.init_state[0] = float(data["stabilizer.roll"])
self.init_state[1] = float(data["stabilizer.pitch"])
self.init_state[2] = data["stabilizer.yaw"]
#self.init_accel_state[0] = data["acc.x"]
#self.init_accel_state[1] = data["acc.y"]
#self.init_accel_state[2] = data["acc.z"]
#self.init_accel_state[3] = data["acc.mag2"]
self.init_gyro_state[0] = data["gyro.x"]
self.init_gyro_state[1] = data["gyro.y"]
self.init_gyro_state[2] = data["gyro.z"]
#self.battery_state = data["pm.vbat"]
#if self.logging_count is 0:
self.altitude[0] = data["baro.asl"]
# self.logging_count += 1
#else:
# self.altitude[0] = float((data["baro.asl"] + self.logging_count*self.altitude[0])/(self.logging_count+1)) #take the running average of the inital altitude for better ground reading
# self.logging_count += 1
#self._cf.altitude[1] = self.altitude[0] #<-----------this shouldnt be here....
#[roll,pitch,yaw,altitude,acc.x,acc.y,acc.z,acc.mag,gyro.x, gyro.y, gyro.z, batteryV]
#print self.altitude
return
self.state[0] = data["stabilizer.roll"]
self.state[1] = data["stabilizer.pitch"]
self.state[2] = data["stabilizer.yaw"]
#self.accel_state[0] = data["acc.x"]
#self.accel_state[1] = data["acc.y"]
#self.accel_state[2] = data["acc.z"]
#self.accel_state[3] = data["acc.mag2"]
self.gyro_state[0] = data["gyro.x"]
self.gyro_state[1] = data["gyro.y"]
self.gyro_state[2] = data["gyro.z"]
#self.battery_state = data["pm.vbat"]
self.altitude[1] = data["baro.asl"]
#print self.altitude
self.next_state[0] = self.state[0] - self.init_state[0]
self.next_state[1] = self.state[1] - self.init_state[0]
def log_file_print(self, file, data):
for i in range(len(data)):
file.write(str(data[i]))
file.write(',')
file.write('\n')
def get_measurements(self):
pass
#return self.measurements
#self.kf = self.kf.em(self.measurements, n_iter=1)
#return self.kf.filter(self.measurements)
def get_gyro(self, value):
if (value == 0):
print ("X value: ", self.gyro_state[0])
elif(value == 1):
print ("Y value: ", self.gyro_state[1])
elif(value == 2):
print ("Z value: ", self.gyro_state[2])
else:
return 0
def get_altitude(self):
#print "altitude: ", self.baro_sensor[0]
return self.altitude[0]
def get_roll(self):
print ("roll: ", self.state[0])
def get_pitch(self):
print ("pitch: ", self.state[1])
def get_yaw(self):
print ("yaw: ", self.state[2])
def get_baro(self, value):
if (value == 0):
print ("lRaw: ", self.baro_sensor[0])
elif(value == 1):
print ("lLong: ", self.baro_sensor[0])
elif(value == 2):
print ("Pressure: ", self.baro_sensor[0])
else:
return 0
class Crazy:
def __init__(self, ui):
# zmienne pomocnicze w obsludze GUI
self.ui = ui
self.thrust = 0
self.pitch = 0
self.roll = 0
self.yaw = 0
self.startup = True
self.is_connected = False
# ustawienia biblioteki cf
self.cf = Crazyflie()
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
#funkcja inicjujaca polaczenie
def connect(self, uri):
self.cf.open_link(uri)
self.is_connected = True
# funkcja wywolywana w chwili odebrania informacji o podlaczeniu sie
def connected(self, uri):
self.ui.l_conn.setText("Connected to {}".format(uri))
print("Connected to {}".format(uri))
control = Thread(target=self.send_ctrl)
control.start()
# self.log_thread()
# funkcja wywolywana w chwili zakonczenia transmisji
def disconnected(self, uri):
self.ui.l_conn.setText("Disconnected")
print("disconnected from {}".format(uri))
self.is_connected = False
# funkcja konczaca polaczenie
def close(self):
self.cf.close_link()
# ustawienia watku zbierajacego dane
def log_thread(self):
self.log = LogConfig(name="logs", period_in_ms=100)
self.log.add_variable("stabilizer.roll", "float")
self.log.add_variable("stabilizer.pitch", "float")
self.log.add_variable("stabilizer.yaw", "float")
try:
self.cf.log.add_config(self.log)
# This callback will receive the data
self.log.data_received_cb.add_callback(self.log_received)
# # This callback will be called on errors
self.log.error_cb.add_callback(self.log_error)
# Start the logging
self.log.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."
# odbieranie danych
def log_received(self, timestamp, data, logconf):
self.katy[0] = data["stabilizer.roll"]
self.katy[1] = data["stabilizer.pitch"]
self.katy[2] = data["stabilizer.yaw"]
self.ui.l_pitch_a.setText("Pitch: {:.3f}".format(self.katy[1]))
self.ui.l_roll_a.setText("Roll: {:.3f}".format(self.katy[0]))
self.ui.l_yaw_a.setText("Yaw: {:.3f}".format(self.katy[2]))
# print("{}, {} = {}".format(timestamp,logconf.name,data["acc.x"]))
def log_error(self, logconf, msg):
print("error while loggoing {}\n{}".format(logconf.name, msg))
# zmiana ustawien sterowania
def update_ctrl(self, thrust, pitch, roll, yaw):
print("thrust changed to {}".format(thrust))
print("yaw changed to {}".format(yaw))
print("pitch changed to {}".format(pitch))
print("roll changed to {}".format(roll))
self.thrust = thrust
self.pitch = pitch
self.roll = roll
self.yaw = yaw
# watek wysylajacy sterowanie
def send_ctrl(self):
while True:
if self.thrust > 60000:
self.thrust = 60000
if self.thrust < 0:
self.thrust = 0
#Unlock startup thrust protection
if self.startup:
self.cf.commander.send_setpoint(0, 0, 0, 0)
self.startup = False
self.cf.commander.send_setpoint(self.roll, self.pitch, self.yaw,
self.thrust)
sleep(0.01)
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=True
def connected(self,uri):
print("Connected to {}".format(uri))
# Thread(target=self.motor).start()
self.log_thread()
def disconnected(self,uri):
print("disconnected from {}".format(uri))
def close(self):
self.cf.close_link()
def log_thread(self):
self.log = LogConfig(name="acc", period_in_ms=100)
self.log.add_variable("acc.x", "float")
self.log.add_variable("acc.y", "float")
self.log.add_variable("acc.z", "float")
try:
self.cf.log.add_config(self.log)
# This callback will receive the data
self.log.data_received_cb.add_callback(self.log_received)
# # This callback will be called on errors
self.log.error_cb.add_callback(self.log_error)
# Start the logging
self.log.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."
t=Timer(5,self.close)
def log_received(self,timestamp, data, logconf):
# self.x.setText("x: {}".format["acc.x"])
# self.y.setText("y: {}".format["acc.y"])
# self.z.setText("z: {}".format["acc.z"])
print("{}, {} = {} on {}".format(timestamp,logconf.name,data["acc.x"],self.uri))
def log_error(self,logconf, msg):
print("error while loggoing {}\n{}".format(logconf.name,msg))
def motor(self):
thrust_mult = 1
thrust_step = 50
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)
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 Quad:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print "Connecting to %s" % link_uri
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print "Connected to %s" % link_uri
Thread(target=self._control).start()
Thread(target=self._input).start()
########################################################################
# Set up Stabilizer Logger
########################################################################
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=50)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._log_stab_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_stab.start()
else:
print(
"Could not add logconfig since some variables are not in TOC")
# ########################################################################
# # Set up Acc Logger
# ########################################################################
# self._lg_acc = LogConfig(name="Acc", period_in_ms=50)
# self._lg_acc.add_variable("acc.x", "float")
# self._lg_acc.add_variable("acc.y", "float")
# self._lg_acc.add_variable("acc.z", "float")
# self._cf.log.add_config(self._lg_acc)
# if self._lg_acc.valid:
# # This callback will receive the data
# self._lg_acc.data_received_cb.add_callback(self._log_acc_data)
# # This callback will be called on errors
# self._lg_acc.error_cb.add_callback(self._log_error)
# # Start the logging
# self._lg_acc.start()
# else:
# print("Could not add logconfig since some variables are not in TOC")
# ########################################################################
# # Set up Gyro Logger
# ########################################################################
# self._lg_gyro = LogConfig(name="Gyro", period_in_ms=50)
# self._lg_gyro.add_variable("gyro.x", "float")
# self._lg_gyro.add_variable("gyro.y", "float")
# self._lg_gyro.add_variable("gyro.z", "float")
# self._cf.log.add_config(self._lg_gyro)
# if self._lg_gyro.valid:
# # This callback will receive the data
# self._lg_gyro.data_received_cb.add_callback(self._log_gyro_data)
# # This callback will be called on errors
# self._lg_gyro.error_cb.add_callback(self._log_error)
# # Start the logging
# self._lg_gyro.start()
# else:
# print("Could not add logconfig since some variables are not in TOC")
# ########################################################################
# # Set up Baro Logger
# ########################################################################
self._lg_baro = LogConfig(name="Baro", period_in_ms=50)
self._lg_baro.add_variable("baro.aslLong", "float")
self._cf.log.add_config(self._lg_baro)
if self._lg_baro.valid:
# This callback will receive the data
self._lg_baro.data_received_cb.add_callback(self._log_baro_data)
# This callback will be called on errors
self._lg_baro.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_baro.start()
else:
print(
"Could not add logconfig since some variables are not in TOC")
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 _log_stab_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global g_roll, g_yaw, g_pitch, g_thrust
(g_roll, g_yaw,
g_pitch) = (data["stabilizer.roll"], data["stabilizer.yaw"],
data["stabilizer.pitch"])
# def _log_acc_data(self, timestamp, data, logconf):
# """Callback froma the log API when data arrives"""
# global g_acc_x, g_acc_y, g_acc_z;
# (g_acc_x, g_acc_y, g_acc_z) = (data["acc.x"], data["acc.y"], data["acc.z"])
# def _log_gyro_data(self, timestamp, data, logconf):
# """Callback froma the log API when data arrives"""
# global g_gyro_x, g_gyro_y, g_gyro_z;
# (g_gyro_x, g_gyro_y, g_gyro_z) = (data["gyro.x"], data["gyro.y"], data["gyro.z"])
def _log_baro_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
global g_baro
g_baro = data["baro.aslLong"]
def _control(self):
global g_roll, g_yaw, g_pitch, g_thrust
global g_gyro_x, g_gyro_y, g_gyro_z
global g_acc_x, g_acc_y, g_acc_z
global g_baro, g_init_baro, g_kill
global diff_baro
global thrust, thrustInit, scaleUp, scaleDown, thrustMin, thrustMax
global count
print "Control Started"
while not g_kill:
#print "acc.x = %10f, acc.y = %10f, acc.z = %10f" % (g_acc_x, g_acc_y, g_acc_z),
#print "gyro.x = %10f, gyro.y = %10f, gyro.z = %10f" % (g_gyro_x, g_gyro_y, g_gyro_z),
print "thrust: %d" % thrust,
print "difference: %10f" % diff_baro,
print "baro = %10f, init_baro = %10f" % (g_baro, g_init_baro),
print "roll = %10f, yaw = %10f, pitch = %10f" % (g_roll, g_yaw,
g_pitch)
if not (g_init_baro == 0):
diff_baro = g_init_baro - g_baro
roll = 0
pitch = -1.5
yawrate = 0
count = count + 1
if diff_baro > 0.0:
thrust = thrustInit + scaleUp * diff_baro
else:
thrust = thrustInit + scaleDown * diff_baro
if thrust < thrustMin:
thrust = thrustMin
if thrust > thrustMax:
thrust = thrustMax
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.01)
# thrust_mult = 1
# thrust_step = 500
# thrust = 20000
# pitch = 0
# roll = 0
# yawrate = 0
# while thrust >= 20000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# if thrust >= 25000:
# thrust_mult = -1
# thrust += thrust_step * thrust_mult
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
self._cf.close_link()
def _input(self):
global g_baro, g_init_baro, g_kill
print "Input Started"
while g_kill is False:
command = raw_input("Input Command:\n")
if (command[0] == 'S') or (command[0] == 's'):
g_init_baro = g_baro - 1
elif (command[0] == 'K') or (command[0] == 'k'):
self._cf.commander.send_setpoint(0, 0, 0, 0)
g_kill = True
print "KILLED!!!"
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 TestFlight:
def __init__(self):
"""
Initialize the quadcopter
"""
self.crazyflie = cflib.crazyflie.Crazyflie()
print 'Initializing drivers'
cflib.crtp.init_drivers()
print 'Searching for available devices'
available = cflib.crtp.scan_interfaces()
radio = False
for i in available:
# Connect to the first device of the type 'radio'
if 'radio' in i[0]:
radio = True
dev = i[0]
print 'Connecting to interface with URI [{0}] and name {1}'.format(i[0], i[1])
self.crazyflie.open_link(dev)
break
if not radio:
print 'No quadcopter detected. Try to connect again.'
exit(-1)
# Set up the callback when connected
self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
"""
Set the loggers
"""
# Log stabilizer
self.logStab = LogConfig("Stabalizer", 200)
self.logStab.add_variable("stabilizer.roll", "float")
self.logStab.add_variable("stabilizer.pitch", "float")
self.logStab.add_variable("stabilizer.yaw", "float")
self.logStab.add_variable("stabilizer.thrust", "uint16_t")
self.crazyflie.log.add_config(self.logStab)
if self.logStab.valid:
self.logStab.data_received_cb.add_callback(self.print_stab_data)
self.logStab.start()
else:
print 'Could not setup log configuration for stabilizer after connection!'
# Log barometer
self.logBaro = LogConfig("Baro", 200)
self.logBaro.add_variable("baro.aslLong", "float")
self.crazyflie.log.add_config(self.logBaro)
if self.logBaro.valid:
self.logBaro.data_received_cb.add_callback(self.print_baro_data)
self.logBaro.start()
else:
print 'Could not setup log configuration for barometer after connection!'
# Log Accelerometer
self.logAccel = LogConfig("Accel",200)
self.logAccel.add_variable("acc.x", "float")
self.logAccel.add_variable("acc.y", "float")
self.logAccel.add_variable("acc.z", "float")
self.crazyflie.log.add_config(self.logAccel)
if self.logAccel.valid:
self.logAccel.data_received_cb.add_callback(self.print_accel_data)
self.logAccel.start()
else:
print 'Could not setup log configuration for accelerometer after connection!'
Thread(target=self.increasing_step).start()
def print_baro_data(self, ident, data, logconfig):
#logging.info("Id={0}, Barometer: asl={1:.4f}".format(ident, data["baro.aslLong"]))
pass
def print_stab_data(self, ident, data, logconfig):
sys.stdout.write('Id={0}, Stabilizer: Roll={1:.2f}, Pitch={2:.2f}, Yaw={3:.2f}, Thrust={4:.2f}\r'.format(ident, data["stabilizer.roll"], data["stabilizer.pitch"], data["stabilizer.yaw"], data["stabilizer.thrust"]))
def print_accel_data(self, ident, data, logconfig):
#logging.info("Id={0}, Accelerometer: x={1:.2f}, y={2:.2f}, z={3:.2f}".format(ident, data["acc.x"], data["acc.y"], data["acc.z"]))
pass
def increasing_step(self):
global key
start_thrust = 10000 # (blades start to rotate after 10000)
min_thrust = 10000
max_thrust = 80000
thrust_increment = 500
start_roll = 0
roll_increment = 30
min_roll = -50
max_roll = 50
start_roll = 0
roll_increment = 30
min_roll = -50
max_roll = 50
start_pitch = 0
pitch_increment = 30
min_pitch = -50
max_pitch = 50
start_yaw = 0
yaw_increment = 30
min_yaw = -200
max_yaw = 200
stop_moving_count = 0
pitch = start_pitch
roll = start_roll
thrust = start_thrust
yaw = start_yaw
# Start the keyread thread
keyreader = KeyReaderThread()
keyreader.start()
sys.stdout.write('\r\nCrazyflie Status\r\n')
sys.stdout.write('================\r\n')
sys.stdout.write("Use 'w' and 's' for the thrust, 'a' and 'd' for yaw, 'i' and 'k' for pitch and 'j' and 'l' for roll. Stop flying with 'q'. Exit with 'e'.\r\n")
while key != "e":
if key == 'q':
thrust = 0
pitch = 0
roll = 0
yaw = 0
elif key == 'w' and (thrust + thrust_increment <= max_thrust):
thrust += thrust_increment
elif key == 's' and (thrust - thrust_increment >= min_roll):
thrust -= thrust_increment
elif key == 'd' and (yaw + yaw_increment <= max_yaw):
yaw += yaw_increment
stop_moving_count = 0
elif key == 'a' and (yaw - yaw_increment >= min_yaw):
yaw -= yaw_increment
stop_moving_count = 0
elif key == 'l' and (roll + roll_increment <= max_roll):
roll += roll_increment
stop_moving_count = 0
elif key == 'j' and (roll - roll_increment >= min_roll):
roll -= roll_increment
stop_moving_count = 0
elif key == 'i' and (pitch + pitch_increment <= max_pitch):
pitch += pitch_increment
stop_moving_count = 0
elif key == 'k' and (pitch - pitch_increment >= min_pitch):
pitch -= pitch_increment
stop_moving_count = 0
elif key == "":
# The controls are not being touch, get back to zero roll, pitch and yaw
if stop_moving_count >= 40:
pitch = 0
roll = 0
yaw = 0
else:
stop_moving_count += 1
else:
pass
key = ""
self.crazyflie.commander.send_setpoint(roll, pitch, yaw, thrust)
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.crazyflie.close_link()
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("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 s
print("Motors ON, starting timer")
t = Timer(3, 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()
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 = 400
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 >= 10000:
self._cf.commander.send_setpoint(0, 0, 0, thrust)
thrust += thrust_step * thrust_mult
time.sleep(0.1)
print ("Landing done")
time.sleep(2)
self._cf.close_link()
def _ramp_motors(self):
thrust = 15000
global landing
landing = False
#print(ref_baro)
# 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.5)
if landing == False:
#self._cf.param.set_value("flightmode.althold","True")
print "Hover ON"
while landing == False:
#thrust = 32767
self._cf.commander.send_setpoint(0,0,0,32767)
self._cf.param.set_value("flightmode.althold","True")
#time.sleep(0.1)
print ("Hovering")
time.sleep(0.1)
print "Hover OFF"
self._cf.param.set_value("flightmode.althold","False")
class Crazy:
def __init__(self,x,y,z,conn_status):
# zmienne pomocnicze w obsludze GUI
self.conn=conn_status
self.x=x
self.y=y
self.z=z
self.thrust=0
self.startup=True
self.is_connected=False
# ustawienia biblioteki cf
self.cf=Crazyflie()
self.cf.connected.add_callback(self.connected)
self.cf.disconnected.add_callback(self.disconnected)
#funkcja inicjujaca polaczenie
def connect(self,uri):
self.cf.open_link(uri)
self.is_connected=True
# funkcja wywolywana w chwili odebrania informacji o podlaczeniu sie
def connected(self,uri):
self.conn.setText("Connected to {}".format(uri))
print("Connected to {}".format(uri))
self.log_thread()
Thread(target=self.send_ctrl).start()
# funkcja wywolywana w chwili zakonczenia transmisji
def disconnected(self,uri):
self.conn.setText("Disconnected")
print("disconnected from {}".format(uri))
self.is_connected=False
# funkcja konczaca polaczenie
def close(self):
self.cf.close_link()
# ustawienia watku zbierajacego dane
def log_thread(self):
self.log = LogConfig(name="acc", period_in_ms=100)
self.log.add_variable("acc.x", "float")
self.log.add_variable("acc.y", "float")
self.log.add_variable("acc.z", "float")
try:
self.cf.log.add_config(self.log)
# This callback will receive the data
self.log.data_received_cb.add_callback(self.log_received)
# # This callback will be called on errors
self.log.error_cb.add_callback(self.log_error)
# Start the logging
self.log.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."
# odbieranie danych
def log_received(self,timestamp, data, logconf):
# self.x.setText("x: {:.3f}".format(data["acc.x"]))
self.y.setText("y: {:.3f}".format(data["acc.y"]))
self.z.setText("z: {:.3f}".format(data["acc.z"]))
# print("{}, {} = {}".format(timestamp,logconf.name,data["acc.x"]))
def log_error(self,logconf, msg):
print("error while loggoing {}\n{}".format(logconf.name,msg))
# zmiana ustawien sterowania
def update_ctrl(self,thrust):
print("thrust changed to {}".format(thrust))
self.thrust=thrust
# watek wysylajacy sterowanie
def send_ctrl(self):
print("ctrl+send")
pitch = 0
roll = 0
yawrate = 0
while True:
thrust=self.thrust
if thrust > 60000:
thrust=60000
if thrust < 0:
thrust=0
#Unlock startup thrust protection
if self.startup:
self.cf.commander.send_setpoint(0,0,0,0)
self.startup=False
self.cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
sleep(0.01)
class Hover:
"""
This will both hover the CF and log/print data concerning the roll,pitch,yaw, etc.
There are 3 separate PID functions to control: pitch, yaw, and roll. Eventually there will be a 4th to control height (or directly: thrust)
"""
def __init__(self, link_uri):
""" The link_uri is the 'radio address' of the crazyflie """
#All of the global variables needed for the PID functions
self.rollsetpoint=0.0
self.pitchsetpoint=0.0
self.yawsetpoint=0.0
self.altitudesetpoint= 20.0
self.thrust=20000
#Calculated errors
self.iroll=0.0
self.droll=0.0
self.ipitch=0.0
self.dpitch=0.0
self.iyaw=0.0
self.dyaw=0.0
self.ialtitude=0.0
self.daltitude=0.0
#The last recorded timestamp (used for dt determination)
self.lastTime=0
self.lastRollError=0.0
self.lastPitchError=0.0
self.lastYawError=0.0
self.lastAltitudeError=0.0
#PID coefficients
self.kp_roll=0.#1.5#1.75
self.kp_pitch=0.#0.75#2.3#2.5
self.kp_yaw=0.0
self.kp_altitude=0.
self.ki_yaw=0.0
self.ki_roll=0.0
self.ki_pitch=0.0#0.000005#0.00000012
self.ki_altitude=0.0
self.kd_roll=0.0
self.kd_pitch=0.0
self.kd_altitude=0.0
self.lastVal=7.0
# Create a Crazyflie object
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
# Connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print "Connected to %s" % link_uri
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="HoverLog", period_in_ms=10)
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.roll","float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._lg_stab.add_variable("stabilizer.thrust", "float")
# self._lg_stab.add_variable("gyro.x","float")
# self._lg_stab.add_variable("gyro.y","float")
#self._lg_stab.add_variable("gyro.z","float")
#self._lg_stab.add_variable("baro.aslLong","float")
#self._lg_stab = LogConfig(name="mb", period_in_ms=10)
self._lg_stab.add_variable("mb.distance","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.
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since some variables are not in TOC")
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Start a timer to disconnect the crazyflie in 25 seconds
t = Timer(25,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)
#The following is all of the PID functions.
#They have been adopted from the general form given in:
#"http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/" """
def pid_roll(self,dataroll,dt):
#Calculate the errors for p,i, and d
error=self.rollsetpoint-dataroll
self.iroll+=(error*dt)
self.droll=(error-self.lastRollError)/dt
#setup for next time
self.lastRollError=error
#return the pid value
return self.kp_roll*error+self.ki_roll*self.iroll+self.kd_roll*self.droll
def pid_pitch(self,datapitch,dt):
#Calculate the errors for p,i, and d
error=self.pitchsetpoint-datapitch
self.ipitch+=(error*dt)
self.dpitch=(error-self.lastPitchError)/dt
#set up the last error for the next iteration
self.lastPitchError=error
#return the pid value
return (self.kp_pitch*error+self.ki_pitch*self.ipitch+self.kd_pitch*self.dpitch)
def pid_yaw(self,datayaw,dt):
#Calculate the errors for p,i, and d
error=self.yawsetpoint-datayaw
self.iyaw+=(error*dt)
self.dyaw=(error-self.lastYawError)/dt
#set up the last error for the next iteration
self.lastYawError=error
#return the pid value
return self.kp_yaw*error+self.ki_yaw*self.iyaw+0*self.dyaw
#Modification to replace Barometer with Sonar Range Finder and optimize thrust using that.
#def thrust_from_sonar(self,dataSonar):
#Based on the "target altitude" argument sent in from the console this is a PID loop based on that setpoint
# def thrust_from_barometer(self,aslLong,dt):
# """old thrust formulation
# base = int(sys.argv[1])
# return 15000+(base-aslLong)*800
# """
# #Calculate the errors for p,i, and d
# error=self.altitudesetpoint-aslLong
# print "error: ",error
# self.ialtitude+=(error*dt)
# self.datitude=(error-self.lastAltitudeError)/dt
# #set up the last error for the next iteration
# self.lastAltitudeError=error
# #return the pid value
# if (((self.kp_altitude*error+self.ki_altitude*self.ialtitude+0*self.daltitude)*200+self.altitudesetpoint*1000)>60000):
# return 55000
# elif (((self.kp_altitude*error+self.ki_altitude*self.ialtitude+0*self.daltitude)*200+self.altitudesetpoint*1000)<20000):
# return 25000
# return (self.kp_altitude*error+self.ki_altitude*self.ialtitude+0*self.daltitude)*200+self.altitudesetpoint*1000
def _stab_log_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives."""
#Print the log data
print "[%d][%s]: %s" % (timestamp, logconf.name, data)
#print self.pid_pitch(data['stabilizer.pitch'],dt)
#calculate the dt for this iteration. Should be close to 10ms
now = timestamp
dt = timestamp-self.lastTime
print (self.altitudesetpoint-data['mb.distance'])*200+(self.altitudesetpoint*1000)+25000
#set the controls of the CF
if (abs(self.altitudesetpoint-data['mb.distance'])>8):
# and \
# (self.lastVal>(((self.altitudesetpoint-data['mb.distance'])*200+(self.altitudesetpoint*1000)+25000)*0.8))and \
# (self.lastVal>(((self.altitudesetpoint-data['mb.distance'])*200+(self.altitudesetpoint*1000)+25000)*0.8)):
self.thrust=(self.altitudesetpoint-data['mb.distance'])*200+(self.altitudesetpoint*1000)+25000
#lif (abs(self.altitudesetpoint-data['mb.distance'])>20):
#self.thrust=(self.altitudesetpoint-data['mb.distance'])*300+(self.altitudesetpoint*1000)+25000
self._cf.commander.send_setpoint(self.pid_roll(data['stabilizer.roll'],dt),-1*self.pid_pitch(data['stabilizer.pitch'],dt),
self.pid_yaw(data['stabilizer.yaw'],dt),self.thrust)
self.lastVal=self.thrust
#set up the lastTime for the next iteration
self.lastTime=now
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print "Connection to %s failed: %s" % (link_uri, msg)
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print "Connection to %s lost: %s" % (link_uri, msg)
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print "Disconnected from %s" % link_uri
self.is_connected = False
class MotorRampExample:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print("Connecting to %s" % link_uri)
def _connected(self, link_uri):
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_sent = 0.0
self._pitch_sent = 0.0
self._yaw_sent = 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")
exit(0)
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('./ramp_accu_log','w')
self._ramp()
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(self):
thrust_mult = 1
thrust_step = 100
thrust = 35000
pitch = 3.95
roll = -5
yawrate = 0
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
while thrust >= 35000:
print("roll: "+str(self._roll_list[0]))
print("pitch: "+str(self._pitch_list[0]))
print("yawrate: "+str(self._yaw_list[0]))
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
if thrust >= 38000:
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()
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 LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print "Connecting to %s" % link_uri
rospy.init_node("angles_and_commander", anonymous = True)
self.pub = rospy.Publisher('angle_sensors', angles)
rospy.Subscriber("control", rpyt, self.ros_callback)
self.roll_rate = 0
self.pitch_rate = 0
self.yaw_rate = 0
self.thrust = 0
self.kill = 0
self.t = 0
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
print "0"
#Set up logging bag
stri = String()
#stri.data = "Alpha: " + str(rospy.get_param('ALPHA')) + " LAG: " + str(rospy.get_param('LAG')) + " VARS: " + str(rospy.get_param('VARS'))
stri.data = 'test'
d = datetime.datetime.now()
path = '/home/cudauser/irobot_quad/quad_cat/logging/'
name = str(d) + "__" + "crazyflie_angles__" + "cmds_motors"
full_name = os.path.join(path, name + ".bag")
self.bag = rosbag.Bag(full_name, 'w')
self.bag.write('metadata', stri)
print "1"
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def ros_callback(self, cmd):
self.roll_rate = cmd.roll_rate
self.pitch_rate = cmd.pitch_rate
self.yaw_rate = cmd.yaw_rate
self.thrust = cmd.thrust
self.kill = max(self.kill, cmd.kill)
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 the motors
#Thread(target=self._ramp_motors).start()
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Sensors", period_in_ms=10)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._lg_stab.add_variable("pm.vbat", "float")
"""
self._lg_stab.add_variable("motor.m1", "float")
self._lg_stab.add_variable("motor.m2", "float")
self._lg_stab.add_variable("motor.m3", "float")
self._lg_stab.add_variable("motor.m4", "float")
print "4"
"""
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since some variables are not in TOC")
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 "Entered callback _stab_log_data()"
s = angles()
roll = data['stabilizer.roll']*np.pi/180.0
pitch = data['stabilizer.pitch']*np.pi/180.0
yaw = data['stabilizer.yaw']*np.pi/180.0
s.roll = roll
s.pitch = pitch
s.yaw = yaw
s.bat = data['pm.vbat']
#s.gyro_x = acceleration[0]
#s.gyro_y = acceleration[1]
#s.gyro_z = acceleration[2]
self.pub.publish(s)
self.bag.write('angles', s)
#self.bag.write('battery', Float32(data['pm.vbat']))
print "Printing kill" + str(self.kill)
if (not rospy.is_shutdown() and self.kill == 0 ):
self.t += 1
mult = 1.0
roll_cmd = self.roll_rate*180/np.pi * 0.5 + 2
pitch_cmd = -(self.pitch_rate*180/np.pi) * 0.5 - 11
yaw_cmd = -(self.yaw_rate*180/np.pi)
thrust_cmd = ((9.81 + self.thrust) - 8.778)/2.4e-05 + 2500
self._cf.commander.send_setpoint(roll_cmd, pitch_cmd, yaw_cmd, thrust_cmd)
# crazy debug block
#roll_cmd = 0
#pitch_cmd = 30
#yaw_cmd = 0
#thrust_cmd = 45000
#print "B"
#if self.t > 2500:
# mult = -1.0
#msg1 = motor_msg()
#msg1.m1 = data['motor.m1']
#msg1.m2 = data['motor.m2']
#msg1.m3 = data['motor.m3']
#msg1.m4 = data['motor.m4']
#msg2 = rpyt()
#msg2.roll_rate = self.roll_rate
#msg2.pitch_rate = self.pitch_rate
#msg2.yaw_rate = self.yaw_rate
#msg2.thrust = self.thrust
#msg2.kill = 0
#self.bag.write('motors', msg1)
#self.bag.write('commands', msg2)
#self.pitch = mult*60
#self.thrust = 40000
#self._cf.commander.send_setpoint(0, self.pitch, 0, self.thrust)
else:
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
self._cf.close_link()
self.bag.close()
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 CrazyTracker:
def __init__(self):
self._log = False;
self._cv = False;
if self._cv:
# Init cameras
self._camera_devices = [0, 1]
self._w = 640
self._h = 480
self._camera = {}
self._pos = {}
self._z_orig = -300;
self._x_orig = 0;
self._y_orig = 0;
# Camera properties
self._clip = 620
for i in self._camera_devices:
self._camera[i] = cv2.VideoCapture(i)
self._camera[i].set(cv2.CAP_PROP_FRAME_WIDTH, self._w)
self._camera[i].set(cv2.CAP_PROP_FRAME_HEIGHT, self._h)
self._pos[i] = ((0,0),0)
self._camera[i].set(cv2.CAP_PROP_EXPOSURE, -6)
# Overlay to see thresholds
self._overlay = np.zeros((self._h,self._w, 3), np.uint8)
self._overlay[:,:,:] = (255, 221, 201)
# Create controlpanel
cv2.namedWindow('Controls')
cv2.createTrackbar('H-Value','Controls',251,255,nothing)
cv2.createTrackbar('H-Range','Controls',15,128,nothing)
cv2.createTrackbar('S-Low','Controls',102,255,nothing)
cv2.createTrackbar('S-High','Controls',255,255,nothing)
cv2.createTrackbar('V-Low','Controls',95,255,nothing)
cv2.createTrackbar('V-High','Controls',245,255,nothing)
cv2.createTrackbar('Gauss','Controls',10,50,nothing)
cv2.createTrackbar('MinSize','Controls',4,96,nothing)
cv2.createTrackbar('Overlay', 'Controls',0,1,nothing)
cv2.createTrackbar('Reset', 'Controls',0,1,nothing)
cv2.createTrackbar('ON/OFF', 'Controls',0,1,nothing)
cv2.createTrackbar('Start', 'Controls',0,1,nothing)
cv2.createTrackbar('Land', 'Controls',0,1,nothing)
cv2.createTrackbar('Cancel', 'Controls',0,1,nothing)
cv2.createTrackbar('Command', 'Controls',0,1,nothing)
cv2.createTrackbar('Param', 'Controls',0,255,nothing)
cv2.resizeWindow('Controls', 350, 300)
self._font = cv2.FONT_HERSHEY_SIMPLEX
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 _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 run(self):
# Init Crazyflier
cflib.crtp.init_drivers(enable_debug_driver=False)
print "Scanning interfaces for Crazyflies..."
available = cflib.crtp.scan_interfaces()
time.sleep(2) #Some USB bugfix
print "Crazyflies found:"
for i in available:
print i[0]
uri = available[-1][0]
cf = Crazyflie()
cf.open_link(uri)
time.sleep(3) #TODO: use the callback instead
cf.commander.send_setpoint(0, 0, 0, 0)
# Init image dictionaries
if self._cv:
img = {}
img_orig = {}
img_blur = {}
img_hsv = {}
mask_hsv = {}
mask_hsv_inv = {}
img_masked = {}
img_tinted = {}
if self._log:
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=100)
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._lg_stab.add_variable("stabilizer.roll", "float")
try:
cf.log.add_config(self._lg_stab)
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._lg_stab.data_received_cb.add_callback(self._stab_log_data)
while True:
# Read controlpanel
h_value = cv2.getTrackbarPos('H-Value', 'Controls')
h_range = cv2.getTrackbarPos('H-Range', 'Controls')
s_low = cv2.getTrackbarPos('S-Low', 'Controls')
s_high = cv2.getTrackbarPos('S-High', 'Controls')
v_low = cv2.getTrackbarPos('V-Low', 'Controls')
v_high = cv2.getTrackbarPos('V-High', 'Controls')
gauss = cv2.getTrackbarPos('Gauss','Controls') * 2 + 1
min_size = cv2.getTrackbarPos('MinSize','Controls') + 1
overlay = cv2.getTrackbarPos('Overlay','Controls')
onoff = cv2.getTrackbarPos('ON/OFF','Controls')
z_target = cv2.getTrackbarPos('Height','Controls')
show_overlay= cv2.getTrackbarPos('Overlay','Controls') == 1
reset = cv2.getTrackbarPos('Reset','Controls') == 1
start = cv2.getTrackbarPos('Start','Controls') == 1
land = cv2.getTrackbarPos('Land','Controls') == 1
cancel = cv2.getTrackbarPos('Cancel','Controls') == 1
command = cv2.getTrackbarPos('Command','Controls') == 1
cmdParam = cv2.getTrackbarPos('Param','Controls')
if start:
cv2.setTrackbarPos('Start','Controls',0)
if land:
cv2.setTrackbarPos('Land','Controls',0)
if cancel:
cv2.setTrackbarPos('Cancel','Controls',0)
if command:
cv2.setTrackbarPos('Command','Controls',0)
#default xyz
sendx = 0
sendy = 0
sendz = 0
# Camera vision code
if self._cv:
h_min = h_value - h_range
h_max = h_value + h_range
for i in self._camera_devices:
self._camera[i].grab()
for i in self._camera_devices:
_, img_orig[i] = self._camera[i].retrieve()
img_blur[i] = cv2.GaussianBlur(img_orig[i], (gauss,gauss), 0)
img_hsv[i] = cv2.cvtColor(img_blur[i], cv2.COLOR_BGR2HSV_FULL)
# Take care of region split for hue (red warps around 255)
if h_min < 0 or h_max > 255:
if h_min < 0:
h_upper = h_min + 255
h_lower = h_max
elif h_max > 255:
h_upper = h_min
h_lower = h_max - 255
mask_lower1 = np.array([h_upper, s_low, v_low],np.uint8)
mask_upper1 = np.array([255, s_high, v_high],np.uint8)
mask_hsv_lower = cv2.inRange(img_hsv[i], mask_lower1, mask_upper1)
mask_lower2 = np.array([0, s_low, v_low],np.uint8)
mask_upper2 = np.array([h_lower, s_high, v_high],np.uint8)
mask_hsv_upper = cv2.inRange(img_hsv[i], mask_lower2, mask_upper2)
mask_hsv[i] = cv2.bitwise_or(mask_hsv_lower, mask_hsv_upper)
else:
mask_lower = np.array([h_min, s_low, v_low],np.uint8)
mask_upper = np.array([h_max, s_high, v_high],np.uint8)
mask_hsv[i] = cv2.inRange(img_hsv[i], mask_lower, mask_upper)
# close and open mask (remove small objects)
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(min_size,min_size))
mask_hsv[i] = cv2.morphologyEx(mask_hsv[i], cv2.MORPH_CLOSE, kernel)
mask_hsv[i] = cv2.morphologyEx(mask_hsv[i], cv2.MORPH_OPEN, kernel)
img_masked[i] = cv2.bitwise_and(img_blur[i], img_blur[i], mask = mask_hsv[i])
#IMPORTANT: This row will change the mask to 0-1 instead of 0-255 and also there will be some countour lines if you invert the mask
_, contours, hierarchy = cv2.findContours(mask_hsv[i], cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
# Find largest contour (we might need to change this to a range)
max_area = 0
largest_contour = None
for idx, contour in enumerate(contours):
area = cv2.contourArea(contour)
if area > max_area:
max_area = area
largest_contour = contour
if not largest_contour == None:
moment = cv2.moments(largest_contour)
self._pos[i] = cv2.minEnclosingCircle(largest_contour)
found = True
else:
found = False
#NOT NICE CODE!! DON'T LOOK HERE!!!
if show_overlay:
mask_hsv_inv[i] = 1 - mask_hsv[i] #Invert now for nice effect
img_tinted[i] = cv2.addWeighted(img_orig[i], 0.2, self._overlay, 0.8, 0)
cv2.bitwise_xor(img_tinted[i], img_tinted[i], img_tinted[i], mask = mask_hsv[i])
cv2.bitwise_xor(img_orig[i], img_orig[i], img_orig[i], mask = mask_hsv_inv[i])
img[i] = cv2.add(img_tinted[i], img_orig[i])
else:
img[i] = img_orig[i]
x = int(self._pos[i][0][0])
y = int(self._pos[i][0][1])
r = int(self._pos[i][1])
if found:
cv2.circle(img[i], (x, y), r, (255, 0, 255), 2)
#fix xyz if found otherwise turn the quad off for security reasons (if cv is disabled the quad will not be turned off)
if found:
x1 = self._pos[0][0][0]
x2 = self._pos[1][0][0]
y1 = self._pos[0][0][1]
y2 = self._pos[1][0][1]
B = 280
D = x2 - x1
f = self._clip
z = f*B/D
x = {}
y = {}
x[0] = (x1-self._w/2)*z/f
y[0] = (self._h-y1)*z/f
x[1] = (x2-self._w/2)*z/f
y[1] = (self._h-y2)*z/f
if (reset):
self._z_orig = z
self._x_orig = x[0]
self._y_orig = y[0]
sendx = int(x[0] - self._x_orig)
sendy = int(z - self._z_orig)
sendz = int(y[0] - self._y_orig)
else:
onoff = False;
# Flags from C-code
#define ON_FLAG 0x1
#define START_FLAG 0x02
#define LAND_FLAG 0x04
#define CANCEL_FLAG 0x08
#define COMMAND_FLAG 0x10
#define PARAM_FLAG 0xFF00
#TODO: maybe change to custom send function (but since this is the exact type of arguments we want we can use it for now i guess)
modeFlags = onoff | start << 1 | land << 2 | cancel << 3 | command << 4 | cmdParam << 8
cf.commander.send_setpoint(sendx, sendy, sendz, modeFlags)
if self._cv:
for i in self._camera_devices:
if not show_overlay and found:
cv2.putText(img[i], 'XYZ:('+`sendx` + ';' + `sendy`+';'+`sendz`+')' , (10, self._h-15), self._font, 1, (0, 255, 255), 1, cv2.LINE_AA)
cv2.imshow("Video" + `i`, img[i])
if cv2.waitKey(1) == 27:
cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
cf.close_link()
cv2.destroyAllWindows()
for i in self._camera_devices:
self._camera[i].release()
break
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 LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print "Connecting to %s" % link_uri
# Try to connect to the Crazyflie
self._cf.open_link("radio://0/2/250K")
#self._cf.open_link("radio://0/8/250K")
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print "Connected to %s" % link_uri
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=10)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since some variables are not in TOC")
Thread(target=self._ramp_motors).start()
# 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 from the log API when data arrives"""
#print "[%d][%s]: %s" % (timestamp, logconf.name, data)
global datas
datas = str(data)
#print datas
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):
global datas
global leap_data
global run_flag
thrust_mult = 1
thrust_step = 2000
thrust = 10000
pitch = 0
del_pitch = 5
roll = 0
del_roll = 5
yawrate = 0
old_leap_data = [0,0,0,0]
old_leap_data[0] = 0
old_leap_data[1] = 0
old_leap_data[2] = 0
old_leap_data[3] = 0
pygame.init()
screen = pygame.display.set_mode((480*2, 360))
name = ""
font = pygame.font.Font(None, 50)
while True:
#print datas
for evt in pygame.event.get():
if evt.type == KEYDOWN:
if evt.key == K_BACKSPACE:
name = name[:-1]
elif evt.key == K_RETURN:
name = ""
elif evt.key == K_LEFT:
roll -= del_roll
#name = str(roll)
elif evt.key == K_RIGHT:
roll += del_roll
#name = str(roll)
elif evt.key == K_DOWN:
pitch -= del_pitch
#name = str(pitch)
elif evt.key == K_UP:
pitch += del_pitch
#name = str(pitch)
elif evt.key == K_w:
thrust += thrust_step
#name = str(thrust)
elif evt.key == K_s:
thrust -= thrust_step
#name = str(thrust)
elif evt.key == K_q:
run_flag = not run_flag
elif evt.key == K_ESCAPE:
pygame.event.post(pygame.event.Event(QUIT))
elif evt.type == QUIT:
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
self._cf.close_link()
return
if abs(leap_data[0]) > 10.0 and abs(leap_data[0])<60.0:
pitch_com = -leap_data[0]/2.0
elif leap_data[0] > 60.0:
pitch_com = -30.0
elif leap_data[0] < -60.0:
pitch_com = 30.0
else:
pitch_com = 0
if abs(leap_data[1]) > 10.0 and abs(leap_data[1])<60.0:
roll_com = -leap_data[1]/2.0
elif leap_data[1]>60.0:
roll_com = -30.0
elif leap_data[1]<-60.0:
roll_com = 30.0
else:
roll_com = 0
if abs(pitch_com) < 10.0 and abs(roll_com) < 10.0 and abs(leap_data[2]) > 20:
pitch_com = 0.0
roll_com = 0.0
yaw_com = leap_data[2]/5.0
else:
yaw_com = 0
if leap_data[3] > 0 and leap_data[3] < 10:
thrust_com = 10000 + leap_data[3] * 2000
if leap_data[3] >=10 and leap_data[3] <=1000:
thrust_com = 29500 + leap_data[3] * 50
elif leap_data[3] <= 0:
thrust_com = 10000
elif leap_data[3] > 1000:
thrust_com = 10000
pitch = (0.707*(roll_com + pitch_com))
roll = (0.707*(roll_com - pitch_com))
yawrate = yaw_com
if run_flag:
thrust = thrust_com
else:
thrust = 10000
#pitch = pitch_com
#roll = roll_com
name = "Pitch: "
name += str(int(pitch_com))
name += ", Roll: "
name += str(int(roll_com))
name += ", Thrust: "
name += str(int(thrust_com))
name += ", Yawrate: "
name += str(int(yaw_com))
name += ", Live: "
name += str(run_flag)
screen.fill ((0, 0, 0))
block = font.render(name, True, (255, 255, 255))
rect = block.get_rect()
rect.center = screen.get_rect().center
screen.blit(block, rect)
pygame.display.flip()
if thrust < 10001:
thrust = 10000
elif thrust > 60000:
thrust = 60000
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
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 logs:
def __init__(self, cf):
#local copy of crazy_Auto
self._cf = cf
# Roll, Pitch, Yaw
self.attitude = [0, 0, 0]
# X, Y, Z
self.position = [0, 0, 0]
# Vx, Vy, Vz
self.velocity = [0, 0, 0]
self._cf._cf.connected.add_callback(self._init_flight_var)
def _init_flight_var(self, link_uri):
print("Connected to %s" % link_uri)
self.RPY_log = LogConfig(name="Stabilizer", period_in_ms=10)
self.RPY_log.add_variable("stabilizer.roll", "float")
self.RPY_log.add_variable("stabilizer.pitch", "float")
self.RPY_log.add_variable("stabilizer.yaw", "int16_t")
self.RPY_log.add_variable('stabilizer.thrust', 'float')
self._cf._cf.log.add_config(self.RPY_log)
self.RPY_log.data_received_cb.add_callback(self.update_attitude)
self.RPY_log.error_cb.add_callback(self.update_error)
self.RPY_log.start()
# self.quaternion.start()
print("Logging Started\n")
# optitrack stuff
self.l_odom = list()
self.l_index = -1
self.sampleInterval = 2
self.s1 = threading.Semaphore(1)
# self.streamingClient = NatNetClient("192.168.1.113") # Net2
self.streamingClient = NatNetClient("172.16.5.205") # Net1
# self.streamingClient.rigidBodyListener = self.receiveRigidBodyFrame
self.streamingClient.rigidBodyListListener = self.receiveRigidBodyFrame
self.streamingClient.run()
time.sleep(1)
self._cf._cf.connected.add_callback(self._cf._connected)
def update_error(self, logconf, msg):
print("Error when logging %s: %s" % (logconf.name, msg))
def update_attitude(self, timestamp, data, logconf):
# print(data)
self.attitude[0] = data["stabilizer.roll"]
self.attitude[1] = data["stabilizer.pitch"]
self.attitude[2] = data["stabilizer.yaw"]
def receiveRigidBodyFrame(self, rigidBodyList, timestamp):
# self.rigidBodyList.append((id, pos, rot, trackingValid))
id = rigidBodyList[0][0]
pos = rigidBodyList[0][1]
rot = rigidBodyList[0][2]
trackingValid = rigidBodyList[0][3]
# print("stamp: ", timestamp)
msg = {'position': [0., 0., 0.], 'stamp': 0, 'velocity': [0., 0., 0.]}
msg['stamp'] = timestamp
msg['position'][0] = pos[0]
msg['position'][1] = pos[1]
msg['position'][2] = pos[2]
self.s1.acquire()
deltatime = 1
if len(self.l_odom) == self.sampleInterval:
last_index = (self.l_index + 2) % self.sampleInterval
last_msg = self.l_odom[last_index]
deltatime = msg['stamp'] - last_msg['stamp']
msg['velocity'][0] = (pos[0] - last_msg['position'][0]) / deltatime
msg['velocity'][1] = (pos[1] - last_msg['position'][1]) / deltatime
msg['velocity'][2] = (pos[2] - last_msg['position'][2]) / deltatime
if abs(msg['velocity'][0]) < 0.0001:
msg['velocity'][0] = 0
if abs(msg['velocity'][1]) < 0.0001:
msg['velocity'][1] = 0
else:
self.l_odom.append(msg)
self.l_index = (self.l_index + 1) % self.sampleInterval
self.l_odom[self.l_index] = msg
self.position[0] = msg['position'][0]
self.position[1] = msg['position'][1]
self.position[2] = msg['position'][2]
self.velocity[0] = msg['velocity'][0]
self.velocity[1] = msg['velocity'][1]
self.velocity[2] = msg['velocity'][2]
self.s1.release()
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 TestFlight:
roll = 0
pitch = 0
yawrate = 0
thrust = 0
hold = "False"
trimmed_roll = 0
trimmed_pitch = 0
def __init__(self):
"""
Initialize the quadcopter
"""
self.f = open('log.log', 'w')
self.starttime = time.time() * 1000.0
self.crazyflie = cflib.crazyflie.Crazyflie()
print 'Initializing drivers'
cflib.crtp.init_drivers()
print 'Searching for available devices'
available = cflib.crtp.scan_interfaces()
radio = False
for i in available:
# Connect to the first device of the type 'radio'
if 'radio' in i[0]:
radio = True
dev = i[0]
print 'Connecting to interface with URI [{0}] and name {1}'.format(
i[0], i[1])
self.crazyflie.open_link(dev)
break
if not radio:
print 'No quadcopter detected. Try to connect again.'
exit(-1)
# Set up the callback when connected
self.crazyflie.connected.add_callback(self.connectSetupFinished)
def connectSetupFinished(self, linkURI):
"""
Set the loggers
"""
# Log stabilizer
self.logStab = LogConfig("Stabalizer", 200)
self.logStab.add_variable("stabilizer.roll", "float")
self.logStab.add_variable("stabilizer.pitch", "float")
self.logStab.add_variable("stabilizer.yaw", "float")
self.logStab.add_variable("stabilizer.thrust", "uint16_t")
self.crazyflie.log.add_config(self.logStab)
if self.logStab.valid:
self.logStab.data_received_cb.add_callback(self.print_stab_data)
self.logStab.start()
else:
print 'Could not setup log configuration for stabilizer after connection!'
Thread(target=self.increasing_step).start()
Thread(target=self.pulse_command).start()
def print_stab_data(self, ident, data, logconfig):
#sys.stdout.write('Stabilizer: Roll={1:.2f}, Pitch={2:.2f}\r'.format(data["stabilizer.roll"], data["stabilizer.pitch"]))
#sys.stdout.flush()
trim_roll = (-1) * data["stabilizer.roll"] + 3.2
trim_pitch = (-1) * data["stabilizer.pitch"] - 0.2
if trim_roll != 0 or trim_pitch != 0:
self.trimmed_roll = self.roll + trim_roll
self.trimmed_pitch = self.pitch + trim_pitch
def increasing_step(self):
while 1:
command = raw_input(
"Set thrust (0-100)% (0 will turn off the motors, e:")
if (command == "e"):
# Exit the main loop
self.hold = "False"
print "Exiting main loop in 1 second"
time.sleep(0.5)
self.crazyflie.close_link(
) # This errors out for some reason. Bad libusb?
elif (command == "h"):
if self.hold == "True":
self.hold = "False"
self.thrust = 42500
print "You're NOT in Hover Mode"
else:
self.hold = "True"
self.thrust = 32767
print "You're in Hover Mode"
elif (self.is_number(command)):
# Good thrust value
self.thrust_a = (int(command))
if self.thrust_a <= 100:
if int(command) == 0:
self.hold = "False"
self.thrust = (int(command) * 555 + 10000)
print "Setting thrust to %i" % (int(command))
else:
print "Remember. Enter a number (0 - 100) or e to exit"
def pulse_command(self):
while 1:
self.crazyflie.param.set_value('flightmode.althold', self.hold)
self.crazyflie.commander.send_setpoint(self.trimmed_roll,
self.trimmed_pitch,
self.yawrate, self.thrust)
time.sleep(0.1)
def is_number(self, s):
try:
int(s)
return True
except ValueError:
return False
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print("Connecting to %s" % link_uri)
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print("Connected to %s" % link_uri)
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=10)
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.log.add_config(self._lg_stab)
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
except KeyError as e:
print("Could not start log configuration,"
"{} not found in TOC".format(str(e)))
except AttributeError:
print("Could not add Stabilizer log config, bad configuration.")
# Start a timer to disconnect in 10s
t = Timer(5, self._cf.close_link)
t.start()
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print("Error when logging %s: %s" % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print("[%d][%s]: %s" % (timestamp, logconf.name, data))
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print("Connection to %s failed: %s" % (link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print("Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print("Disconnected from %s" % link_uri)
self.is_connected = False
class LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 5s.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Crazyflie object without specifying any cache dirs
self._cf = Crazyflie()
# Connect some callbacks from the Crazyflie API
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print("Connecting to %s" % link_uri)
self._configured = False
self._exit = False
# Try to connect to the Crazyflie
self._cf.open_link(link_uri)
raw_input("Configuring magnometer. Press any key to stop\n")
self._configured = True
raw_input("Press any key to stop\n")
self._exit = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print("Connected to %s" % link_uri)
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name="Stabilizer", period_in_ms=10)
'''
self._lg_stab.add_variable("stabilizer.roll", "float")
self._lg_stab.add_variable("stabilizer.pitch", "float")
self._lg_stab.add_variable("stabilizer.yaw", "float")
self._lg_stab.add_variable("baro.aslRaw", "float")
'''
self._lg_stab.add_variable("mag.x", "float")
self._lg_stab.add_variable("mag.y", "float")
self._lg_stab.add_variable("mag.z", "float")
self._min_x = self._min_y = self._min_z = 99.9
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
self._cf.log.add_config(self._lg_stab)
if self._lg_stab.valid:
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
else:
print("Could not add logconfig since some variables are not in TOC")
Thread(target=self._work).start()
def _work(self):
print("Work starting")
while not self._configured or not self._exit:
time.sleep(0.1)
print("Work stopped")
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 from the log API when data arrives"""
# print "[%d][%s]: %s" % (timestamp, logconf.name, data)
if not self._configured:
self._min_x = min(data["mag.x"], self._min_x)
self._min_y = min(data["mag.y"], self._min_y)
self._min_z = min(data["mag.z"], self._min_z)
print("\r({x:=8},{y:=8},{z:=8})) [{x_min:=8},{y_min:=8},{z_min:=8}]"\
.format(\
x=data["mag.x"],\
y=data["mag.y"],\
z=data["mag.z"],\
x_min=self._min_x,\
y_min=self._min_y,\
z_min=self._min_z),end="")
else:
print("\r({x},{y},{z})"\
.format(\
x=data["mag.x"] - self._min_x,\
y=data["mag.y"] - self._min_y,\
z=data["mag.z"] - self._min_z),end="")
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)
