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!")
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 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 _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 _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("pm.vbat", "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(5, 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 connectSetupFinished(self, linkURI):
"""
Configure the logger to log baroerometer values and start recording.
The logging variables are added one after another to the logging
configuration. Then the configuration is used to create a log packet
which is cached on the Crazyflie. If the log packet is None, the
program exits. Otherwise the logging packet receives a callback when
it receives data, which prints the data from the logging packet's
data dictionary as logging info.
"""
# Set baroerometer logging config
baro_log_conf = LogConfig("baro", 10)
baro_log_conf.addVariable(LogVariable("baro.asl", "float"))
baro_log_conf.addVariable(LogVariable("baro.aslRaw", "float"))
baro_log_conf.addVariable(LogVariable("baro.aslLong", "float"))
baro_log_conf.addVariable(LogVariable("baro.temp", "float"))
baro_log_conf.addVariable(LogVariable("baro.pressure", "float"))
# Now that the connection is established, start logging
self.baro_log = self.crazyflie.log.create_log_packet(baro_log_conf)
if self.baro_log is not None:
self.baro_log.dataReceived.add_callback(self.log_baro_data)
self.baro_log.start()
else:
print("baro.stuffs not found in log TOC")
def _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 _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print("Connected to %s" % link_uri)
# The definition of the logconfig can be made before connecting
self._lg_stabilizer = LogConfig(name="stabilizer", period_in_ms=10)
self._lg_stabilizer.add_variable("stabilizer.roll", "float")
self._lg_stabilizer.add_variable("stabilizer.thrust", "uint16_t")
self._lg_stabilizer.add_variable("stabilizer.yaw", "float")
self._lg_stabilizer.add_variable("stabilizer.pitch", "float")
# Adding the configuration cannot be done until a Crazyflie is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._cf.log.add_config(self._lg_stabilizer)
# This callback will receive the data
self._lg_stabilizer.data_received_cb.add_callback(self._stabilizer_log_data)
# This callback will be called on errors
self._lg_stabilizer.error_cb.add_callback(self._stabilizer_log_error)
# Start the logging
self._lg_stabilizer.start()
except KeyError as e:
print("Could not start log configuration,"
"{} not found in TOC".format(str(e)))
except AttributeError:
print("Could not add stabilizer log config, bad configuration.")
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._hover).start()
def 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):
input_thread = Thread(target=self.input_loop)
input_thread.start()
lg = LogConfig ("Battery", 1000)
lg.addVariable(LogVariable("pm.vbat", "float"))
log = self.crazyflie.log.create_log_packet(lg)
if (log != None):
log.dataReceived.add_callback(self.batteryData)
log.start()
else:
print "battery not found in log TOC"
stabilizerconf = LogConfig("Stabilizer", 1000)
stabilizerconf.addVariable(LogVariable("stabilizer.thrust", "uint16_t"))
stabilizerconf.addVariable(LogVariable("stabilizer.yaw", "float"))
stabilizerconf.addVariable(LogVariable("stabilizer.roll", "float"))
stabilizerconf.addVariable(LogVariable("stabilizer.pitch", "float"))
stabilizerlog = self.crazyflie.log.create_log_packet(stabilizerconf)
if (stabilizerlog != None):
stabilizerlog.dataReceived.add_callback(self.stabilizerData)
stabilizerlog.start()
else:
print "stabilizer not found in log TOC"
accelconf = LogConfig("Accel", 1000)
accelconf.addVariable(LogVariable("acc.x", "float"))
accelconf.addVariable(LogVariable("acc.y", "float"))
accelconf.addVariable(LogVariable("acc.z", "float"))
# stabilizerconf.addVariable(LogVariable("stabilizer.pitch", "float"))
accellog = self.crazyflie.log.create_log_packet(accelconf)
if (accellog != None):
accellog.dataReceived.add_callback(self.accelData)
accellog.start()
else:
print "accelerometer not found in log TOC"
print "ready"
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
rospy.loginfo("Connected to %s" % link_uri)
self._state = CrazyflieROS.Connected
#self._lg_imu = LogConfig(name="IMU", period_in_ms=10)
#self._lg_imu.add_variable("acc.x", "float")
#self._lg_imu.add_variable("acc.y", "float")
#self._lg_imu.add_variable("acc.z", "float")
#self._lg_imu.add_variable("gyro.x", "float")
#self._lg_imu.add_variable("gyro.y", "float")
#self._lg_imu.add_variable("gyro.z", "float")
#self._cf.log.add_config(self._lg_imu)
#if self._lg_imu.valid:
# This callback will receive the data
# self._lg_imu.data_received_cb.add_callback(self._log_data_imu)
# This callback will be called on errors
# self._lg_imu.error_cb.add_callback(self._log_error)
# Start the logging
# self._lg_imu.start()
#else:
# rospy.logfatal("Could not add logconfig since some variables are not in TOC")
self._lg_log2 = LogConfig(name="LOG2", period_in_ms=100)
self._lg_log2.add_variable("mag.x", "float")
self._lg_log2.add_variable("mag.y", "float")
self._lg_log2.add_variable("mag.z", "float")
self._lg_log2.add_variable("baro.temp", "float")
self._lg_log2.add_variable("baro.pressure", "float")
self._lg_log2.add_variable("pm.vbat", "float")
self._cf.log.add_config(self._lg_log2)
if self._lg_log2.valid:
# This callback will receive the data
self._lg_log2.data_received_cb.add_callback(self._log_data_log2)
# This callback will be called on errors
self._lg_log2.error_cb.add_callback(self._log_error)
# Start the logging
self._lg_log2.start()
else:
rospy.logfatal("Could not add logconfig since some variables are not in TOC")
p_toc = self._cf.param.toc.toc
for group in p_toc.keys():
self._cf.param.add_update_callback(group=group, name=None, cb=self._param_callback)
for name in p_toc[group].keys():
ros_param = "~{}/{}".format(group, name)
cf_param = "{}.{}".format(group, name)
if rospy.has_param(ros_param):
self._cf.param.set_value(cfparam, rospy.get_param(ros_param))
else:
self._cf.param.request_param_update(cf_param)
def _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 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 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 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 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 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 _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 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!")
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 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):
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 _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):
""" 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 setup_log(self):
""" Console callbacks """
self.crazyflie.console.receivedChar.add_callback(self.consoleCB)
rospy.sleep(0.25)
""" ATTITUDE LOGGING @ 100hz """
logconf = LogConfig("attitude", self.HZ100 * 2) #ms
logconf.addVariable(LogVariable("attitude.q0", "float"))
logconf.addVariable(LogVariable("attitude.q1", "float"))
logconf.addVariable(LogVariable("attitude.q2", "float"))
logconf.addVariable(LogVariable("attitude.q3", "float"))
self.logAttitude = self.crazyflie.log.create_log_packet(logconf)
if (self.logAttitude is not None):
self.logAttitude.dataReceived.add_callback(
self.logCallbackAttitude)
self.logAttitude.error.add_callback(self.logErrorCB)
if self.attitude_monitor:
self.logAttitude.start()
rospy.loginfo("Attitude Logging started")
else:
rospy.logwarn("Could not setup Attitude logging!")
rospy.sleep(0.25)
""" ZPID LOGGING @ 100hz """
logconf = LogConfig("zpid", self.HZ100) #ms
logconf.addVariable(LogVariable("zpid.p", "float"))
logconf.addVariable(LogVariable("zpid.i", "float"))
logconf.addVariable(LogVariable("zpid.d", "float"))
logconf.addVariable(LogVariable("zpid.pid", "float"))
self.logZPid = self.crazyflie.log.create_log_packet(logconf)
if (self.logZPid is not None):
self.logZPid.dataReceived.add_callback(self.logCallbackZPid)
self.logZPid.error.add_callback(self.logErrorCB)
if self.zpid_monitor:
self.logZPid.start()
rospy.loginfo("ZPID Logging started")
else:
rospy.logwarn("Could not setup ZPID logging!")
rospy.sleep(0.25)
""" HOVER LOGGING @ 100hz """
logconf = LogConfig("hover", self.HZ100) #ms
logconf.addVariable(LogVariable("hover.err", "float"))
logconf.addVariable(LogVariable("hover.target", "float"))
logconf.addVariable(LogVariable("hover.zSpeed", "float"))
logconf.addVariable(LogVariable("hover.zBias", "float"))
logconf.addVariable(LogVariable("hover.acc_vspeed", "float"))
logconf.addVariable(LogVariable("hover.asl_vspeed", "float"))
self.logHover = self.crazyflie.log.create_log_packet(logconf)
if (self.logHover is not None):
self.logHover.dataReceived.add_callback(self.logCallbackHover)
self.logHover.error.add_callback(self.logErrorCB)
if self.hover_monitor:
self.logHover.start()
rospy.loginfo("Hover Logging started")
else:
rospy.logwarn("Could not setup Hover logging!")
rospy.sleep(0.25)
""" GYROMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingGyro", self.HZ100) #ms
logconf.addVariable(LogVariable("gyro.x", "float"))
logconf.addVariable(LogVariable("gyro.y", "float"))
logconf.addVariable(LogVariable("gyro.z", "float"))
self.logGyro = self.crazyflie.log.create_log_packet(logconf)
if (self.logGyro is not None):
self.logGyro.dataReceived.add_callback(self.logCallbackGyro)
self.logGyro.error.add_callback(self.logErrorCB)
if self.gyro_monitor:
self.logGyro.start()
rospy.loginfo("Gyro Logging started")
else:
rospy.logwarn("Could not setup Gyro logging!")
rospy.sleep(0.25)
""" ACCELEROMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingAcc", self.HZ100) #ms
logconf.addVariable(LogVariable("acc.x", "float"))
logconf.addVariable(LogVariable("acc.y", "float"))
logconf.addVariable(LogVariable("acc.z", "float"))
logconf.addVariable(LogVariable("acc.xw", "float"))
logconf.addVariable(LogVariable("acc.yw", "float"))
logconf.addVariable(LogVariable("acc.zw", "float"))
self.logAcc = self.crazyflie.log.create_log_packet(logconf)
if (self.logAcc is not None):
self.logAcc.dataReceived.add_callback(self.logCallbackAcc)
self.logAcc.error.add_callback(self.logErrorCB)
if self.acc_monitor:
self.logAcc.start()
rospy.loginfo("Acc Logging started")
else:
rospy.logwarn("Could not setup Acc logging!")
rospy.sleep(0.25)
""" MAGNETOMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingMag", self.HZ100) #ms
logconf.addVariable(LogVariable("mag.x", "float"))
logconf.addVariable(LogVariable("mag.y", "float"))
logconf.addVariable(LogVariable("mag.z", "float"))
logconf.addVariable(LogVariable("mag.x_raw", "float"))
logconf.addVariable(LogVariable("mag.y_raw", "float"))
logconf.addVariable(LogVariable("mag.z_raw", "float"))
self.logMag = self.crazyflie.log.create_log_packet(logconf)
if (self.logMag is not None):
self.logMag.dataReceived.add_callback(self.logCallbackMag)
self.logMag.error.add_callback(self.logErrorCB)
if self.mag_monitor:
self.logMag.start()
rospy.loginfo("Mag Logging started")
else:
rospy.logwarn("Could not setup Mag logging!")
rospy.sleep(0.25)
""" BAROMETER LOGGING @ 100hz """
logconf = LogConfig("LoggingBaro", self.HZ100) #ms
logconf.addVariable(LogVariable("baro.aslRaw", "float"))
logconf.addVariable(LogVariable("baro.asl", "float"))
logconf.addVariable(LogVariable("baro.temp", "float"))
logconf.addVariable(LogVariable("baro.pressure", "float"))
logconf.addVariable(LogVariable("baro.aslLong", "float"))
self.logBaro = self.crazyflie.log.create_log_packet(logconf)
if (self.logBaro is not None):
self.logBaro.dataReceived.add_callback(self.logCallbackBaro)
self.logBaro.error.add_callback(self.logErrorCB)
if self.baro_monitor:
self.logBaro.start()
rospy.loginfo("Baro Logging started")
else:
rospy.logwarn("Could not setup Baro logging!")
rospy.sleep(0.25)
""" BATTERY/LINK LOGGING @ 10hz """
logconf = LogConfig("LoggingBat", self.HZ10) #ms
logconf.addVariable(LogVariable("pm.vbat", "float"))
logconf.addVariable(LogVariable("pm.state", "uint8_t"))
logconf.addVariable(LogVariable("pm.state_charge", "uint8_t"))
self.logBat = self.crazyflie.log.create_log_packet(logconf)
if (self.logBat is not None):
self.logBat.dataReceived.add_callback(self.logCallbackBat)
self.logBat.error.add_callback(self.logErrorCB)
if self.bat_monitor:
self.logBat.start()
rospy.loginfo("Bat Logging started")
else:
rospy.logwarn("Could not setup Battery/Link logging!")
rospy.sleep(0.25)
#TODO motor logging not working
""" MOTOR LOGGING @ 100hz"""
logconf = LogConfig("LoggingMotor", self.HZ100) #ms
logconf.addVariable(LogVariable("motor.m1", "uint32_t"))
logconf.addVariable(LogVariable("motor.m2", "uint32_t"))
logconf.addVariable(LogVariable("motor.m3", "uint32_t"))
logconf.addVariable(LogVariable("motor.m4", "uint32_t"))
logconf.addVariable(LogVariable("motor.thrust", "uint16_t"))
logconf.addVariable(LogVariable("motor.vLong", "float")) #TODO move
self.logMotor = self.crazyflie.log.create_log_packet(logconf)
if (self.logMotor is not None):
self.logMotor.dataReceived.add_callback(self.logCallbackMotor)
self.logMotor.error.add_callback(self.logErrorCB)
if self.motor_monitor:
self.logMotor.start()
rospy.loginfo("Motor Logging started")
else:
rospy.logwarn("Could not setup Motor logging!")
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()
