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 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 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 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)