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 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 createConfigFromSelection(self):
logconfig = LogConfig(str(self.configNameCombo.currentText()), self.period)
for node in self.getNodeChildren(self.varTree.invisibleRootItem()):
parentName = node.text(NAME_FIELD)
for leaf in self.getNodeChildren(node):
varName = leaf.text(NAME_FIELD)
varType = str(leaf.text(CTYPE_FIELD))
completeName = "%s.%s" % (parentName, varName)
newVar = LogVariable(completeName, fetchAs=varType, storedAs=varType)
logconfig.addVariable(newVar)
return logconfig
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_motors = LogConfig(name="Motors", period_in_ms=100)
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_motors.add_variable("motor.m1", "int32_t")
self._lg_motors.add_variable("motor.m2", "int32_t")
self._lg_motors.add_variable("motor.m3", "int32_t")
self._lg_motors.add_variable("motor.m4", "int32_t")
try:
self._cf.log.add_config(self._lg_stab)
self._cf.log.add_config(self._lg_motors)
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()
self._lg_motors.data_received_cb.add_callback(self._motors_log_data)
self._lg_motors.error_cb.add_callback(self._stab_log_error)
self._lg_motors.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 _init_logs(self):
accel_log_conf = LogConfig("Accel", 10)
accel_log_conf.addVariable(LogVariable("acc.x", "float"))
accel_log_conf.addVariable(LogVariable("acc.y", "float"))
accel_log_conf.addVariable(LogVariable("acc.z", "float"))
self.accel_log = self.crazyflie.log.create_log_packet(accel_log_conf)
if self.accel_log is not None:
self.accel_log.dataReceived.add_callback(self.log_accel_data)
self.accel_log.start()
else:
print("acc.x/y/z not found in log TOC")
def connectionDone(self, linkURI):
self.setUIState(UIState.CONNECTED, linkURI)
GuiConfig().set("link_uri", linkURI)
lg = LogConfig("Battery", 1000)
lg.addVariable(LogVariable("pm.vbat", "float"))
self.log = self.cf.log.create_log_packet(lg)
if self.log != None:
self.log.dataReceived.add_callback(self.batteryUpdatedSignal.emit)
self.log.error.add_callback(self.loggingError)
self.log.start()
else:
logger.warning("Could not setup loggingblock!")
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 _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", 1000)
self._log_sensor.add_variable("altHold.err")
self._log_sensor.add_variable("altHold.target")
#self._cf.log.add_config(self._log_sensor)
try:
self._cf.log.add_config(self._log_sensor)
# 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()
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 s
print("Motors ON, starting timer")
# Start a timer to disconnect in 10s
t = Timer(5, 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()
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 _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 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."
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 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."
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 connectSetupFinished(self, linkURI):
"""
Set the loggers
"""
# 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!'
# 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 connectSetupFinished(self, linkURI):
lg = LogConfig("Magnetometer", 100)
lg.addVariable(LogVariable("mag.x", "int16_t"))
lg.addVariable(LogVariable("mag.y", "int16_t"))
lg.addVariable(LogVariable("mag.z", "int16_t"))
log = self.crazyflie.log.create_log_packet(lg)
if log is not None:
log.dataReceived.add_callback(self.magData)
log.start()
else:
print "Magnetometer not found in log TOC"
# Keep the commands alive so the firmware kill-switch doesn't kick in.
Thread(target=self.pulse_command).start()
Thread(target=self.input_loop).start()
def 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 _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 setupStabilizerLog(self):
log_conf = LogConfig("Pitch", 10)
log_conf.addVariable(LogVariable("stabilizer.pitch", "float"))
log_conf.addVariable(LogVariable("stabilizer.roll", "float"))
log_conf.addVariable(LogVariable("stabilizer.thrust", "int32_t"))
log_conf.addVariable(LogVariable("stabilizer.yaw", "float"))
self.pitch_log = self.crazyflie.log.create_log_packet(log_conf)
if self.pitch_log is not None:
self.pitch_log.dataReceived.add_callback(self.log_pitch_data)
self.pitch_log.start()
print("stabilizer.pitch/roll/thrust/yaw now logging")
else:
print("stabilizer.pitch/roll/thrust/yaw not found in log TOC")
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 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."
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 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 _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 register_yaw_update_callback(self):
logconf = LogConfig("stabilizer", 10)
logconf.addVariable(LogVariable("stabilizer.pitch", "float"))
logconf.addVariable(LogVariable("stabilizer.roll", "float"))
logconf.addVariable(LogVariable("stabilizer.yaw", "float"))
self.logPacket = self.crazyflie.log.create_log_packet(logconf)
if (self.logPacket is None):
logger.warning(
"Could not setup logconfiguration after connection!")
return
self.logPacket.dataReceived.add_callback(self.on_stabilizer_update)
self.logPacket.error.add_callback(self.on_yaw_update_callback_failed)
self.logPacket.start()
def _set_available_sensors(self, name, available):
logger.info("[%s]: %s", name, available)
available = eval(available)
if ("HMC5883L" in name):
if (not available):
self.actualASL.setText("N/A")
self.actualASL.setEnabled(False)
else:
self.actualASL.setEnabled(True)
self.helper.inputDeviceReader.setAltHoldAvailable(available)
if (not self.logBaro and not self.logAltHold):
# The sensor is available, set up the logging
lg = LogConfig("Baro", 200)
lg.addVariable(LogVariable("baro.aslLong", "float"))
self.logBaro = self.helper.cf.log.create_log_packet(lg)
if (self.logBaro is not None):
self.logBaro.data_received.add_callback(self._baro_data_signal.emit)
self.logBaro.error.add_callback(self._log_error_signal.emit)
self.logBaro.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
lg = LogConfig("AltHold", 200)
lg.addVariable(LogVariable("altHold.target", "float"))
self.logAltHold = self.helper.cf.log.create_log_packet(lg)
if (self.logAltHold is not None):
self.logAltHold.data_received.add_callback(self._althold_data_signal.emit)
self.logAltHold.error.add_callback(self._log_error_signal.emit)
self.logAltHold.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
def connectSetupFinished(self, linkURI):
"""
Configure the logger to log accelerometer values and start recording.
The logging variables are added one after another to the logging
configuration. Then the configuration is used to create a log packet
which is cached on the Crazyflie. If the log packet is None, the
program exits. Otherwise the logging packet receives a callback when
it receives data, which prints the data from the logging packet's
data dictionary as logging info.
"""
# Set stabilizer logging config
acc_log_conf = LogConfig("acc", 10)
acc_log_conf.addVariable(LogVariable("acc.x", "float"))
acc_log_conf.addVariable(LogVariable("acc.y", "float"))
acc_log_conf.addVariable(LogVariable("acc.z", "float"))
# Now that the connection is established, start logging
self.acc_log = self.crazyflie.log.create_log_packet(acc_log_conf)
if self.acc_log is not None:
self.acc_log.dataReceived.add_callback(self.log_acc_data)
self.acc_log.start()
else:
print("acc.x/y/z not found in log TOC")
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 _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 _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 register_yaw_update_callback(self):
logconf = LogConfig("stabilizer", 10)
logconf.addVariable(LogVariable("stabilizer.pitch", "float"))
logconf.addVariable(LogVariable("stabilizer.roll", "float"))
logconf.addVariable(LogVariable("stabilizer.yaw", "float"))
self.logPacket = self.crazyflie.log.create_log_packet(logconf)
if (self.logPacket is None):
logger.warning("Could not setup logconfiguration after connection!")
return
self.logPacket.dataReceived.add_callback(self.on_stabilizer_update)
self.logPacket.error.add_callback(self.on_yaw_update_callback_failed)
self.logPacket.start()
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
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 __init__(self, link_uri):
self._cf = Crazyflie()
#adding the logging parameter
print("Connecting to %s" % link_uri)
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._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)
def _init_logs(self):
accel_log_conf = LogConfig("Accel", 10)
accel_log_conf.addVariable(LogVariable("acc.x", "float"))
accel_log_conf.addVariable(LogVariable("acc.y", "float"))
accel_log_conf.addVariable(LogVariable("acc.z", "float"))
self.accel_log = self.crazyflie.log.create_log_packet(accel_log_conf)
if self.accel_log is not None:
self.accel_log.dataReceived.add_callback(self.log_accel_data)
self.accel_log.start()
else:
print("acc.x/y/z not found in log TOC")
def connectSetupFinished(self, linkURI):
# Set accelerometer logging config
motor_log_conf = LogConfig("motor", 10)
motor_log_conf.addVariable(LogVariable("motor.m4", "int32_t"))
motor_log_conf.addVariable(LogVariable("motor.m1", "int32_t"))
motor_log_conf.addVariable(LogVariable("motor.m2", "int32_t"))
motor_log_conf.addVariable(LogVariable("motor.m3", "int32_t"))
# Now that the connection is established, start logging
self.motor_log = self.crazyflie.log.create_log_packet(motor_log_conf)
if self.motor_log is not None:
self.motor_log.dataReceived.add_callback(self.log_motor_data)
self.motor_log.start()
else:
print("motor.m1/m2/m3/m4 not found in log TOC")
self.crazyflie.close_link()
def _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)
self._lg_acc.data_received_cb.add_callback(self._acc_log_data)
self._lg_acc.error_cb.add_callback(self._acc_log_error)
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 connectSetupFinished(self, linkURI):
lg = LogConfig("Magnetometer", 100)
lg.addVariable(LogVariable("mag.x", "int16_t"))
lg.addVariable(LogVariable("mag.y", "int16_t"))
lg.addVariable(LogVariable("mag.z", "int16_t"))
log = self.crazyflie.log.create_log_packet(lg)
if log is not None:
log.dataReceived.add_callback(self.magData)
log.start()
else:
print "Magnetometer not found in log TOC"
# Keep the commands alive so the firmware kill-switch doesn't kick in.
Thread(target=self.pulse_command).start()
Thread(target=self.input_loop).start()
def gyro_log(self):
gyro_log_file = open(self.gyro_log_filename, 'wb')
self.gyro_writer = csv.writer(gyro_log_file)
self.gyro_writer.writerow(['time','gyro.x','gyro.y','gyro.z'])
gyro_log_config = LogConfig('gyro', 1000/log_freq)
gyro_log_config.addVariable(LogVariable('gyro.x', 'float'))
gyro_log_config.addVariable(LogVariable('gyro.y', 'float'))
gyro_log_config.addVariable(LogVariable('gyro.z', 'float'))
self.gyro_log = self.crazyflie.log.create_log_packet(gyro_log_config)
if (self.gyro_log is not None):
self.gyro_log.data_received.add_callback(self.gyro_data)
self.gyro_log.error.add_callback(self.logging_error)
self.gyro_log.start()
else:
logger.warning("Could not setup logconfiguration after connection!")
def connectSetupFinished(self, linkURI):
# Set accelerometer logging config
motor_log_conf = LogConfig("motor", 10)
motor_log_conf.addVariable(LogVariable("motor.m4", "int32_t"))
motor_log_conf.addVariable(LogVariable("motor.m1", "int32_t"))
motor_log_conf.addVariable(LogVariable("motor.m2", "int32_t"))
motor_log_conf.addVariable(LogVariable("motor.m3", "int32_t"))
# Now that the connection is established, start logging
self.motor_log = self.crazyflie.log.create_log_packet(motor_log_conf)
if self.motor_log is not None:
self.motor_log.dataReceived.add_callback(self.log_motor_data)
self.motor_log.start()
else:
print("motor.m1/m2/m3/m4 not found in log TOC")
self.crazyflie.close_link()
def motor_log(self):
motor_log_file = open(self.motor_log_filename, 'wb')
self.motor_writer = csv.writer(motor_log_file)
self.motor_writer.writerow(['time','m1','m2','m3','m4'])
motor_log_config = LogConfig('motor', 1000/log_freq)
motor_log_config.addVariable(LogVariable('motor.m1', 'uint32_t'))
motor_log_config.addVariable(LogVariable('motor.m2', 'uint32_t'))
motor_log_config.addVariable(LogVariable('motor.m3', 'uint32_t'))
motor_log_config.addVariable(LogVariable('motor.m4', 'uint32_t'))
self.motor_log = self.crazyflie.log.create_log_packet(motor_log_config)
if (self.motor_log is not None):
self.motor_log.data_received.add_callback(self.motor_data)
self.motor_log.error.add_callback(self.logging_error)
self.motor_log.start()
else:
logger.warning("Could not setup logconfiguration after connection!")
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 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 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 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
def connected(self, linkURI):
lg = LogConfig("Stabalizer", 100)
lg.addVariable(LogVariable("stabilizer.roll", "float"))
lg.addVariable(LogVariable("stabilizer.pitch", "float"))
lg.addVariable(LogVariable("stabilizer.yaw", "float"))
lg.addVariable(LogVariable("stabilizer.thrust", "uint16_t"))
self.log = self.helper.cf.log.create_log_packet(lg)
if (self.log is not None):
self.log.dataReceived.add_callback(self._imu_data_signal.emit)
self.log.error.add_callback(self.loggingError)
self.log.start()
else:
logger.warning(
"Could not setup logconfiguration after connection!")
lg = LogConfig("Motors", 100)
lg.addVariable(LogVariable("motor.m1", "uint32_t"))
lg.addVariable(LogVariable("motor.m2", "uint32_t"))
lg.addVariable(LogVariable("motor.m3", "uint32_t"))
lg.addVariable(LogVariable("motor.m4", "uint32_t"))
self.log = self.helper.cf.log.create_log_packet(lg)
if (self.log is not None):
self.log.dataReceived.add_callback(self._motor_data_signal.emit)
self.log.error.add_callback(self.loggingError)
self.log.start()
else:
logger.warning(
"Could not setup logconfiguration after connection!")
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()
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()
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 _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)
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 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 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 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 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 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:
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)