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 _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):
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"
print "ready"
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):
"""
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 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 _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 _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 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 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 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):
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!")
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 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 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 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):
# 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):
rospy.loginfo("Connected on %s"%linkURI)
#set up logging
period_msec = 10
acc_conf = LogConfig("Accel", period_msec)
acc_conf.addVariable(LogVariable("acc.x", "float"))
acc_conf.addVariable(LogVariable("acc.y", "float"))
acc_conf.addVariable(LogVariable("acc.z", "float"))
ori_conf = LogConfig("Stabilizer", period_msec)
ori_conf.addVariable(LogVariable("stabilizer.pitch", "float"))
ori_conf.addVariable(LogVariable("stabilizer.yaw", "float"))
ori_conf.addVariable(LogVariable("stabilizer.roll", "float"))
ori_conf.addVariable(LogVariable("stabilizer.thrust", "float"))
#Set up acc logging
self.acc_log = self.crazyflie.log.create_log_packet(acc_conf)
self.orientation_log = self.crazyflie.log.create_log_packet(ori_conf)
if (self.acc_log != None):
self.acc_log.dataReceived.add_callback(self.acc_data_callback)
self.acc_log.start()
rospy.loginfo("Succesfully set up acc logging")
else:
rospy.logerr("Unable to set up acc logging")
if (self.orientation_log != None):
self.orientation_log.dataReceived.add_callback(self.orientation_data_callback)
self.orientation_log.start()
rospy.loginfo("Succesfully set up stabilizer logging")
else:
rospy.logerr("Unable to set up stabilizer logging")
def 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 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 connected(self, linkURI):
# IMU & THRUST
lg = LogConfig("Stabalizer", 200)
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.data_received.add_callback(self._imu_data_signal.emit)
self.log.error.add_callback(self._log_error_signal.emit)
self.log.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
# MOTOR
lg = LogConfig("Motors", 200)
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.data_received.add_callback(self._motor_data_signal.emit)
self.log.error.add_callback(self._log_error_signal.emit)
self.log.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
# HEADING
lg = LogConfig("Heading", 200)
lg.addVariable(LogVariable("heading.x", "int16_t"))
lg.addVariable(LogVariable("heading.y", "int16_t"))
lg.addVariable(LogVariable("heading.z", "int16_t"))
self.log = self.helper.cf.log.create_log_packet(lg)
if (self.log is not None):
self.log.data_received.add_callback(self._heading_data_signal.emit)
self.log.error.add_callback(self._log_error_signal.emit)
self.log.start()
else:
logger.warning("Could not setup logconfiguration after "
"connection!")
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, 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 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"
