class Main: """ Class is required so that methods can access the object fields. """ def __init__(self): """ Connect to Crazyflie, initialize drivers and set up callback. The callback takes care of logging the accelerometer values. """ self.crazyflie = Crazyflie() cflib.crtp.init_drivers() self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) self.crazyflie.open_link("radio://0/8/250K") 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 log_motor_data(self,data): thrust_mult = 3 thrust_step = 500 thrust = 20000 pitch = 0 roll = 0 yawrate = 0 while thrust >= 20000: self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if (thrust >= 50000): time.sleep(.01) thrust_mult = -1 thrust = thrust + (thrust_step * thrust_mult) self.crazyflie.commander.send_setpoint(0,0,0,0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) def log_motor_data(self, data): logging.info("Motors: m1=%.2f, m2=%.2f, m3=%.2f, m4=%.2f" % (data["motor.m1"], data["motor.m2"], data["motor.m3"], data["motor.m4"]))
class Driver: # Initial values, you can use these to set trim etc. roll = 0.0 pitch = 0.0 yawrate = 0 thrust = 10001 def __init__(self, options=None): self.crazyflie = Crazyflie() cflib.crtp.init_drivers() rospy.loginfo("Waiting for crazyflie...") while 1: interfaces = cflib.crtp.scan_interfaces() if len(interfaces)>1: radio = interfaces[0][0] rospy.loginfo("Found: " + radio) self.crazyflie.open_link(radio) break else: rospy.sleep(1) self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): rospy.loginfo("Connected to " + linkURI)
class Main: def __init__(self): self._stop = 0 self.crazyflie = Crazyflie() cflib.crtp.init_drivers() # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link("radio://0/10/250K") self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) 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 stop(self): # Exit command received # Set thrust to zero to make sure the motors turn off NOW # make sure we actually set the thrust to zero before we kill the thread that sets it time.sleep(0.5) self._stop = 1; # Exit the main loop time.sleep(1) self.crazyflie.close_link() # This errors out for some reason. Bad libusb? sys.exit(0) def input_loop(self): command = raw_input("") self.stop() def pulse_command(self): while 1: self.crazyflie.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) # Exit if the parent told us to if self._stop==1: return def magData(self, data): x, y, z = data['mag.x'], data['mag.y'], data['mag.z'] try: print x, y, z sys.stdout.flush() except: self.stop()
class Main: def __init__(self, uri): self.crazyflie = Crazyflie() self.uri = uri cflib.crtp.init_drivers() # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link(uri) # Set up the callback when connected self.crazyflie.connected.add_callback(self.connectSetupFinished) def connectSetupFinished(self, linkURI): # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self.pulse_command).start() def pulse_command(self): thrust_mult = 1 thrust_step = 500 thrust = 20000 pitch = 0 roll = 0 yawrate = 0 while thrust >= 20000: self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if thrust >= 25000: thrust_mult = -1 thrust += thrust_step * thrust_mult self.crazyflie.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) self.crazyflie.close_link()
class AltHold: 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): Thread(target=self._ramp_motors).start() def _connection_failed(self, link_uri, msg): print("Connection to %s failed: %s" % (link_uri, msg)) def _connection_lost(self, link_uri, msg): print("Connection to %s lost: %s" % (link_uri, msg)) def _disconnected(self, link_uri): print("Disconnected from %s" % link_uri) def _ramp_motors(self): thrust = 36000 #Works at JoBa pitch = 0 roll = 0 yawrate = 10 self._cf.commander.send_setpoint(0, 0, 0, 0) print("Constant thrust") self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.75) print("Turning on altitude hold") self._cf.param.set_value("flightmode.althold", "True") start_time = time.time() timeC =time.time() - start_time while timeC<3: #wait for 3 seconds self._cf.commander.send_setpoint(0, 0, 0, 32767) time.sleep(0.1) print("hovering!") timeC =time.time() - start_time print("Closing link, time taken was: ") print timeC #close link and execute landing self._cf.commander.send_setpoint(0, 0, 0, 0) time.sleep(0.1) self._cf.close_link()
class HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache", rw_cache=sys.path[1]+"/cache") signal.signal(signal.SIGINT, signal.SIG_DFL) def setup_controller(self, input_config, input_device=0): """Set up the device reader""" # Set values for input from config (advanced) self._jr.set_thrust_limits( self._p2t(Config().get("min_thrust")), self._p2t(Config().get("max_thrust"))) self._jr.set_rp_limit(Config().get("max_rp")) self._jr.set_yaw_limit(Config().get("max_yaw")) self._jr.set_thrust_slew_limiting( self._p2t(Config().get("slew_rate")), self._p2t(Config().get("slew_limit"))) # Set up the joystick reader self._jr.device_error.add_callback(self._input_dev_error) devs = self._jr.getAvailableDevices() print "Will use [%s] for input" % devs[input_device]["name"] self._jr.start_input(devs[input_device]["name"], input_config) def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print "Controller #{}: {}".format(dev["id"], dev["name"]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie on the given link uri""" self._cf.connectionFailed.add_callback(self._connection_failed) self._cf.open_link(link_uri) self._jr.input_updated.add_callback(self._cf.commander.send_setpoint) def _connection_failed(self, link, message): """Callback for a failed Crazyflie connection""" print "Connection failed on {}: {}".format(link, message) self._jr.stop_input() sys.exit(-1) def _input_dev_error(self, message): """Callback for an input device error""" print "Error when reading device: {}".format(message) sys.exit(-1) def _p2t(self, percentage): """Convert a percentage to raw thrust""" return int(65000 * (percentage / 100.0))
class MotorRampExample: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.open_link(link_uri) print "Connecting to %s" % link_uri def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._ramp_motors).start() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)""" print "Connection to %s failed: %s" % (link_uri, msg) def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print "Connection to %s lost: %s" % (link_uri, msg) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print "Disconnected from %s" % link_uri def _ramp_motors(self): thrust_mult = 1 thrust_step = 500 thrust = 20000#20000 thrustmax = 45000#25000 pitch = 0 roll = 0 yawrate = 0 while thrust >= 20000: print "Thrust: "+str(round(thrust,2)) self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if thrust >= thrustmax: 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()
class HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache", rw_cache=sys.path[1]+"/cache") signal.signal(signal.SIGINT, signal.SIG_DFL) def setup_controller(self, input_config, input_device=0, xmode=False): """Set up the device reader""" # Set up the joystick reader self._jr.device_error.add_callback(self._input_dev_error) print "Client side X-mode: %s" % xmode if (xmode): self._cf.commander.set_client_xmode(xmode) devs = self._jr.getAvailableDevices() print "Will use [%s] for input" % devs[input_device]["name"] self._jr.start_input(devs[input_device]["name"], input_config) def controller_connected(self): """ Return True if a controller is connected""" return True if (len(self._jr.getAvailableDevices()) > 0) else False def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print "Controller #{}: {}".format(dev["id"], dev["name"]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie on the given link uri""" self._cf.connection_failed.add_callback(self._connection_failed) self._cf.param.add_update_callback(group="imu_sensors", name="HMC5883L", cb=(lambda name, found: self._jr.setAltHoldAvailable(eval(found)))) self._jr.althold_updated.add_callback( lambda enabled: self._cf.param.set_value("flightmode.althold", enabled)) self._cf.open_link(link_uri) self._jr.input_updated.add_callback(self._cf.commander.send_setpoint) def _connection_failed(self, link, message): """Callback for a failed Crazyflie connection""" print "Connection failed on {}: {}".format(link, message) self._jr.stop_input() sys.exit(-1) def _input_dev_error(self, message): """Callback for an input device error""" print "Error when reading device: {}".format(message) sys.exit(-1)
class MotorRampExample: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.open_link(link_uri) print "Connecting to %s" % link_uri def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._ramp_motors).start() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)""" print "Connection to %s failed: %s" % (link_uri, msg) def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print "Connection to %s lost: %s" % (link_uri, msg) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print "Disconnected from %s" % link_uri def _ramp_motors(self): thrust_mult = 1 thrust_step = 500 #m1list = [20000,25000,30000,35000,40000,45000,50000,55000,60000] m1list = [20000] m2 = 0 m3 = 0 m4 = 0 for m1 in m1list: for i in range(2000): time.sleep(0.01) self._cf.commander.send_setpoint(0, 45, 0, m1) 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(5) self._cf.close_link()
class Controller: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.open_link(link_uri) print "Connecting to %s" % link_uri def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" print "Connected." #self._quadcopter = Quadcopter (self._cf) # The older, harder-to-use code self._quadcopter = QuadcopterAltHold (self._cf) # Altitude hold mode! Thread(target=self._run).start() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the specified address)""" print "Connection to %s failed: %s" % (link_uri, msg) def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print "Connection to %s lost: %s" % (link_uri, msg) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print "Disconnected from %s" % link_uri def _quality_updated(self, quality): if quality < 30: print "Low link quality: %s", quality def _run(self): print "Running..." current_time = 0 self._cf.commander.send_setpoint (0,0,0,0); while current_time <= 5: print current_time self._quadcopter._update(self._cf) time.sleep(0.1) current_time += 0.1 self._cf.close_link() print "Done."
class Main: def __init__(self, uri): self.crazyflie = Crazyflie() self.uri = uri cflib.crtp.init_drivers(enable_debug_driver=False) # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link(uri) # Set up the callback when connected self.crazyflie.connected.add_callback(self.connectSetupFinished) self.crazyflie.disconnected.add_callback(self._disconnected) self.crazyflie.connection_failed.add_callback(self._connection_failed) self.crazyflie.connection_lost.add_callback(self._connection_lost) def connectSetupFinished(self, linkURI): # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self.pulse_command).start() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the specified address)""" print("Connection to %s failed: %s" % (link_uri, msg)) def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print("Connection to %s lost: %s" % (link_uri, msg)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print("Disconnected from %s" % link_uri) def pulse_command(self): thrust_mult = 1 thrust_step = 500 thrust = 20000 pitch = 0 roll = 0 yawrate = 0 # Unlock startup thrust protection print("unlock crazyflie") self.crazyflie.commander.send_setpoint(0, 0, 0, 0) while thrust >= 20000: self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) time.sleep(0.1) if thrust >= 25000: thrust_mult = -1 thrust += thrust_step * thrust_mult self.crazyflie.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) print("command Ended") self.crazyflie.close_link()
class Main: """ Class is required so that methods can access the object fields. """ def __init__(self): """ Connect to Crazyflie, initialize drivers and set up callback. The callback takes care of logging the accelerometer values. """ self.crazyflie = Crazyflie() cflib.crtp.init_drivers() self.crazyflie.connectSetupFinished.add_callback( self.connectSetupFinished) self.crazyflie.open_link("radio://0/8/250K") 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 accelerometer logging config 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")) accel_log_conf.addVariable(LogVariable("gyro.x", "float")) accel_log_conf.addVariable(LogVariable("gyro.y", "float")) accel_log_conf.addVariable(LogVariable("gyro.z", "float")) # Now that the connection is established, start logging 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 log_accel_data(self, data): logging.info("Accelerometer: x=%.2f, y=%.2f, z=%.2f; Gyrometer: x=%.2f, y=%.2f, z=%.2f" % (data["acc.x"], data["acc.y"], data["acc.z"], data["gyro.x"], data["gyro.y"], data["gyro.z"]))
class LpsRebootToBootloader: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.open_link(link_uri) print('Connecting to %s' % link_uri) def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._reboot_thread).start() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the specified address)""" print('Connection to %s failed: %s' % (link_uri, msg)) def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print('Connection to %s lost: %s' % (link_uri, msg)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print('Disconnected from %s' % link_uri) def _reboot_thread(self): anchors = LoPoAnchor(self._cf) print('Sending reboot signal to all anchors 10 times in a row ...') for retry in range(10): for anchor_id in range(6): anchors.reboot(anchor_id, anchors.REBOOT_TO_BOOTLOADER) time.sleep(0.1) self._cf.close_link()
class Main: """ Class is required so that methods can baroess the object fields. """ def __init__(self): """ Connect to Crazyflie, initialize drivers and set up callback. The callback takes care of logging the baroerometer values. """ self.crazyflie = Crazyflie() cflib.crtp.init_drivers() self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished) self.crazyflie.open_link("radio://0/10/250K") def connectSetupFinished(self, linkURI): """ Configure the logger to log baroerometer values and start recording. The logging variables are added one after another to the logging configuration. Then the configuration is used to create a log packet which is cached on the Crazyflie. If the log packet is None, the program exits. Otherwise the logging packet receives a callback when it receives data, which prints the data from the logging packet's data dictionary as logging info. """ # Set baroerometer logging config baro_log_conf = LogConfig("baro", 10) baro_log_conf.addVariable(LogVariable("baro.asl", "float")) baro_log_conf.addVariable(LogVariable("baro.aslRaw", "float")) baro_log_conf.addVariable(LogVariable("baro.aslLong", "float")) baro_log_conf.addVariable(LogVariable("baro.temp", "float")) baro_log_conf.addVariable(LogVariable("baro.pressure", "float")) # Now that the connection is established, start logging self.baro_log = self.crazyflie.log.create_log_packet(baro_log_conf) if self.baro_log is not None: self.baro_log.dataReceived.add_callback(self.log_baro_data) self.baro_log.start() else: print("baro.stuffs not found in log TOC") def log_baro_data(self, data): logging.info( "Barometer: asl=%.2f, aslRaw=%.2f, aslLong=%.2f, temp=%.2f, pressure=%.2f" % (data["baro.asl"], data["baro.aslRaw"], data["baro.aslLong"], data["baro.temp"], data["baro.pressure"]) )
def __init__(self,parent): super(Crazy, self).__init__() # zmienne pomocnicze w obsludze GUI self.thrust=0 self.pitch=0 self.roll=0 self.yaw=0 self.startup=True self.is_connected=False self.busy=False self.control_started=False # zmienna do wysylania logow self.log_data={"roll":0.0,"pitch":0.0,"yaw":0.0,"thrust":0.0,"m1":0.0,"m2":0.0,"m3":0.0,"m4":0.0} # ustawienia dobierania danych do sterowania parent.control_data_sig.connect(self.update_ctrl_sig) # ustawienia biblioteki cf self.cf=Crazyflie() self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) self.cf.connection_lost.add_callback(self.lost_connection) #ustawienie watku sterowania self.control=Thread(target=self.send_ctrl)
def __init__(self, link_uri): self.nthrust = 0 self.nroll = 0 self.npitch = 0 self.nyaw = 0 self.init_time = 0 self.time_elapsed = 0 # to plot graphics self.thrust_v = [] self.roll_v = [] self.pitch_v = [] self.yaw_v = [] self.time_v = [] # 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) self.is_connected = True
def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ # Create a Crazyflie object without specifying any cache dirs self._cf = Crazyflie() # Connect some callbacks from the Crazyflie API self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) print("Connecting to %s" % link_uri) self._configured = False self._exit = False # Try to connect to the Crazyflie self._cf.open_link(link_uri) raw_input("Configuring magnometer. Press any key to stop\n") self._configured = True raw_input("Press any key to stop\n") self._exit = True
def __init__(self, link_uri): # Set Initial values # 43000 @ 3700mV # 47000 @ 3500mV - not enough self._thrust = 10000 self._rollTrim = 0 self._pitchTrim = 0 self._rollThrustFactor = 150 self._pitchTrustFactor = 250 self._maxBatteryCounter = 50 self._batteryCounter = 0 self._batteryVoltage = 4000 #about full """ Initialize and run the example with the specified link_uri """ print "Connecting to %s" % link_uri self._cf = Crazyflie(ro_cache="./crazyflie-clients-python/lib/cflib/cache", rw_cache="./crazyflie-clients-python/lib/cflib/cache") 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.is_connected = False self._cf.open_link(link_uri) print "Connecting to %s" % link_uri
def __init__(self, link_uri): self._runCouner = 0 self._firstYawFound = False # Set Initial values self._thrust = 41000 self._pitch = 0 self._roll = 0 self._yawrate = 0 self._yawSetPoint = 0 self._rollPid = PID() self._rollPid.SetKp(-.1); # Proportional Gain #self._rollPid.SetKi(-0.5); # Integral Gain #self._rollPid.SetKd(-0.1); # Derivative Gain """ Initialize and run the example with the specified link_uri """ print "Connecting to %s" % link_uri self._cf = Crazyflie(ro_cache="./crazyflie-clients-python/lib/cflib/cache", rw_cache="./crazyflie-clients-python/lib/cflib/cache") 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 # Variable used to keep main loop occupied until disconnect self.is_connected = True
def on_pose(self, myo, timestamp, pose): if pose == libmyo.Pose.double_tap: #Throttle myo.vibrate('short') self.holder = 0 print("tap") elif pose == libmyo.Pose.fist: #Calibrate roll and pitch myo.vibrate('short') self.cal = 0 print("fist") elif pose == libmyo.Pose.wave_out: #Disconnect myo.vibrate('short') self._cf.commander.send_setpoint(0, 0, 0, 0) #Clear packets time.sleep(0.1) self._cf.close_link() print("wave_out") elif pose == libmyo.Pose.wave_in: #Connect myo.vibrate('short') 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(self.link) print("wave_in") self.pose = pose
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.link = link_uri self._cf = Crazyflie() self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.open_link(link_uri) self.thrust_h = 0 self.holder = 0 self.cal = 0 self.swap = False self.quat = libmyo.Quaternion(0,0,0,1) print("Connecting to %s" % link_uri)
def initCF(self): self.cfConnected = False self.cf = None self.cfUri = None logger.debug("Initializing Drivers; Debug=FALSE") InitDrivers(enable_debug_driver=False) logger.info("Scanning") available = Scan() logger.info("Found:") for cf in available: logger.debug(cf[0]) if len(available) > 0: self.cfUri = available[0][0] self.cf = Crazyflie() self.cf.connected.add_callback(self.cfOnConnected) self.cf.disconnected.add_callback(self.cfOnDisconnected) self.cf.connection_failed.add_callback(self.cfOnFailed) self.cf.connection_lost.add_callback(self.cfOnLost) logger.info("Crazyflie @{uri} Initialized".format(uri=self.cfUri)) else: logger.info("No Crazyflies found")
def __init__(self, link_uri, tf_prefix, roll_trim, pitch_trim, enable_logging): self.link_uri = link_uri self.tf_prefix = tf_prefix self.roll_trim = roll_trim self.pitch_trim = pitch_trim self.enable_logging = enable_logging self._cf = Crazyflie() self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.link_quality_updated.add_callback(self._link_quality_updated) self._cmdVel = Twist() self._subCmdVel = rospy.Subscriber(tf_prefix + "/cmd_vel", Twist, self._cmdVelChanged) self._pubImu = rospy.Publisher(tf_prefix + "/imu", Imu, queue_size=10) self._pubTemp = rospy.Publisher(tf_prefix + "/temperature", Temperature, queue_size=10) self._pubMag = rospy.Publisher(tf_prefix + "/magnetic_field", MagneticField, queue_size=10) self._pubPressure = rospy.Publisher(tf_prefix + "/pressure", Float32, queue_size=10) self._pubBattery = rospy.Publisher(tf_prefix + "/battery", Float32, queue_size=10) self._state = CrazyflieROS.Disconnected rospy.Service(tf_prefix + "/update_params", UpdateParams, self._update_params) rospy.Service(tf_prefix + "/emergency", Empty, self._emergency) self._isEmergency = False Thread(target=self._update).start()
def __init__(self): self.crazyflie = Crazyflie() cflib.crtp.init_drivers() self.targetParameters = {"forward_angle": 0, "left_angle": 0, "thrust_percentage": 0} self.currentParameters = {"pitch": 0, "roll": 0, "yaw": 0} self.frameOfReference = {"pitch": 0, "roll": 0, "yaw": 0}
def __init__(self, URI): QtGui.QMainWindow.__init__(self) self.resize(700, 500) self.setWindowTitle('Multi-ranger point cloud') self.canvas = Canvas(self.updateHover) self.canvas.create_native() self.canvas.native.setParent(self) self.setCentralWidget(self.canvas.native) cflib.crtp.init_drivers(enable_debug_driver=False) self.cf = Crazyflie(ro_cache=None, rw_cache='cache') # Connect callbacks from the Crazyflie API self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) # Connect to the Crazyflie self.cf.open_link(URI) self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3} self.hoverTimer = QtCore.QTimer() self.hoverTimer.timeout.connect(self.sendHoverCommand) self.hoverTimer.setInterval(100) self.hoverTimer.start()
def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ # UDP socket for interfacing with GCS self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self._sock.bind(('127.0.0.1', 14551)) # 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 threading.Thread(target=self._server).start()
def __init__(self): self.crazyflie = Crazyflie() cflib.crtp.init_drivers() # You may need to update this value if your Crazyradio uses a different frequency. self.crazyflie.open_link("radio://0/9/250K") # Set up the callback when connected self.crazyflie.connectSetupFinished.add_callback(self.connectSetupFinished)
def __init__(self, link_uri): """ Initializes the control class and executes all needed functions. """ # Connect Crazyflie self.Crazyflie = Crazyflie() self.Connected = False self.Connect(link_uri) while not self.Connected: wait(0.1) pass # Start Program self.t0 = 0#getTime() self.Running = True # Initialize self.SetInitialState() self.InitializeReferenceCS() if Plotting: self.InitializePlotting() if GUI: self.InitializeGUI() if Animation: self.InitializeAnimation() # Run Main Loops Thread(target=self.MainLoop).start() if GUI: Thread(target=self.GUILoop).start() if Animation: Thread(target=self.AnimationLoop).start()
def __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 __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 is_close(range): MIN_DISTANCE = 0.2 # m if range is None: return False else: return range < MIN_DISTANCE if __name__ == '__main__': # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf=cf) as scf: with MotionCommander(scf) as motion_commander: with Multiranger(scf) as multiranger: keep_flying = True while keep_flying: VELOCITY = 0.5 velocity_x = 0.0 velocity_y = 0.0 if is_close(multiranger.front): velocity_x -= VELOCITY if is_close(multiranger.back): velocity_x += VELOCITY
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") self._lg_stab.add_variable("pm.vbat", "float") # Adding the configuration cannot be done until a Crazyflie is # connected, since we need to check that the variables we # would like to log are in the TOC. self._cf.log.add_config(self._lg_stab) if self._lg_stab.valid: # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._stab_log_data) # This callback will be called on errors self._lg_stab.error_cb.add_callback(self._stab_log_error) # Start the logging self._lg_stab.start() else: print( "Could not add logconfig since some variables are not in TOC") # Start a timer to disconnect in 10s t = Timer(5, self._cf.close_link) t.start() def _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
# if len(available) == 0: # print('No Crazyflies found, cannot run example') # else: #lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) #lg_stab.add_variable('stabilizer.roll', 'float') #lg_stab.add_variable('stabilizer.pitch', 'float') #lg_stab.add_variable('stabilizer.yaw', 'float') lg_stab = LogConfig(name='stateEstimate', period_in_ms=10) lg_stab.add_variable('stateEstimate.x', 'float') lg_stab.add_variable('stateEstimate.y', 'float') lg_stab.add_variable('stateEstimate.z', 'float') x_arr = np.array([]) y_arr = np.array([]) t = np.array([]) cf = Crazyflie(rw_cache='./cache') #Connecting to radio with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf #DEFAULTS cf.param.set_value('posCtlPid.zKp', '{:.2f}'.format(2)) cf.param.set_value('posCtlPid.zKi', '{:.2f}'.format(0.5)) cf.param.set_value('posCtlPid.zKd', '{:.2f}'.format(0)) #New Values kp = 2 ki = 0.5 kd = 0 # cf.param.set_value('posCtlPid.xKp','{:.2f}'.format(kp)) # cf.param.set_value('posCtlPid.xKi','{:.2f}'.format(ki)) # cf.param.set_value('posCtlPid.xKd','{:.2f}'.format(kd)) # cf.param.set_value('posCtlPid.yKp','{:.2f}'.format(kp))
logging.basicConfig(level=logging.ERROR) def stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print('Error when logging %s: %s' % (logconf.name, msg)) def stab_log_data(self, timestamp, data, logconf): """Callback from a the log API when data arrives""" print('[%d][%s]: %s' % (timestamp, logconf.name, data)) if __name__ == '__main__': cflib.crtp.init_drivers() cf = Crazyflie(rw_cache="./cache") cf.open_link(link_uri=URI) lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) lg_stab.add_variable('stabilizer.roll', 'float') lg_stab.add_variable('stabilizer.pitch', 'float') lg_stab.add_variable('stabilizer.yaw', 'float') try: cf.log.add_config(lg_stab) # This callback will receive the data lg_stab.data_received_cb.add_callback(stab_log_data) # This callback will be called on errors lg_stab.error_cb.add_callback(stab_log_error) # Start the logging lg_stab.start()
class RadioBridge: def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ # UDP socket for interfacing with GCS self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self._sock.bind(('127.0.0.1', 14551)) # Create a Crazyflie object without specifying any cache dirs self._cf = Crazyflie() # Connect some callbacks from the Crazyflie API self._cf.link_established.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 threading.Thread(target=self._server).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._cf.packet_received.add_callback(self._got_packet) def _got_packet(self, pk): if pk.port == CRTP_PORT_MAVLINK: self._sock.sendto(pk.data, ('127.0.0.1', 14550)) def _forward(self, data): pk = CRTPPacket() pk.port = CRTP_PORT_MAVLINK # CRTPPort.COMMANDER pk.data = data # struct.pack('<fffH', roll, -pitch, yaw, thrust) self._cf.send_packet(pk) def _server(self): while True: print('\nwaiting to receive message') # Only receive what can be sent in one message data, address = self._sock.recvfrom(256) print('received %s bytes from %s' % (len(data), address)) for i in range(0, len(data), 30): self._forward(data[i:(i + 30)]) def _stab_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print('Error when logging %s: %s' % (logconf.name, msg)) def _stab_log_data(self, timestamp, data, logconf): """Callback from a 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 specified 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 Flasher(object): """ A class that can flash the DS28E05 EEPROM via CRTP. """ def __init__(self, link_uri): self._cf = Crazyflie() self._link_uri = link_uri # Add 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) # Initialize variables self.connected = False # Public methods def connect(self): """ Connect to the crazyflie. """ print('Connecting to %s' % self._link_uri) self._cf.open_link(self._link_uri) def disconnect(self): print('Disconnecting from %s' % self._link_uri) self._cf.close_link() def wait_for_connection(self, timeout=10): """ Busy loop until connection is established. Will abort after timeout (seconds). Return value is a boolean, whether connection could be established. """ start_time = datetime.datetime.now() while True: if self.connected: return True now = datetime.datetime.now() if (now - start_time).total_seconds() > timeout: return False time.sleep(0.5) def search_memories(self): """ Search and return list of 1-wire memories. """ if not self.connected: raise NotConnected() return self._cf.mem.get_mems(MemoryElement.TYPE_1W) # Callbacks def _connected(self, link_uri): print('Connected to %s' % link_uri) self.connected = True def _disconnected(self, link_uri): print('Disconnected from %s' % link_uri) self.connected = False def _connection_failed(self, link_uri, msg): print('Connection to %s failed: %s' % (link_uri, msg)) self.connected = False def _connection_lost(self, link_uri, msg): print('Connection to %s lost: %s' % (link_uri, msg)) self.connected = False
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(tf_prefix + "/cmd_vel", Twist, self._cmdVelChanged) self._pubImu = rospy.Publisher(tf_prefix + "/imu", Imu, queue_size=10) self._pubTemp = rospy.Publisher(tf_prefix + "/temperature", Temperature, queue_size=10) self._pubMag = rospy.Publisher(tf_prefix + "/magnetic_field", MagneticField, queue_size=10) self._pubPressure = rospy.Publisher(tf_prefix + "/pressure", Float32, queue_size=10) self._pubBattery = rospy.Publisher(tf_prefix + "/battery", Float32, queue_size=10) self._state = CrazyflieROS.Disconnected rospy.Service(tf_prefix + "/update_params", UpdateParams, self._update_params) 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("pm.state", "uint8_t") self._cf.log.add_config(self._lg_log2) if self._lg_log2.valid: # This callback will receive the data self._lg_log2.data_received_cb.add_callback(self._log_data_log2) # This callback will be called on errors self._lg_log2.error_cb.add_callback(self._log_error) # Start the logging self._lg_log2.start() else: rospy.logfatal( "Could not add logconfig since some variables are not in TOC") # self._lg_log3 = LogConfig(name="LOG3", period_in_ms=100) # self._lg_log3.add_variable("motor.m1", "int32_t") # self._lg_log3.add_variable("motor.m2", "int32_t") # self._lg_log3.add_variable("motor.m3", "int32_t") # self._lg_log3.add_variable("motor.m4", "int32_t") # self._lg_log3.add_variable("stabalizer.pitch", "float") # self._lg_log3.add_variable("stabalizer.roll", "float") # self._lg_log3.add_variable("stabalizer.thrust", "uint16_") # self._lg_log3.add_variable("stabalizer.yaw", "float") # # self._cf.log.add_config(self._lg_log3) #if self._lg_log3.valid: # # This callback will receive the data # self._lg_log3.data_received_cb.add_callback(self._log_data_log3) # # This callback will be called on errors # self._lg_log3.error_cb.add_callback(self._log_error) # # Start the logging # self._lg_log3.start() #else: # rospy.logfatal("Could not add logconfig since some variables are not in TOC") p_toc = self._cf.param.toc.toc for group in p_toc.keys(): self._cf.param.add_update_callback(group=group, name=None, cb=self._param_callback) for name in p_toc[group].keys(): ros_param = "/{}/{}/{}".format(self.tf_prefix, group, name) cf_param = "{}.{}".format(group, name) if rospy.has_param(ros_param): self._cf.param.set_value(cf_param, rospy.get_param(ros_param)) else: self._cf.param.request_param_update(cf_param) def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)""" rospy.logfatal("Connection to %s failed: %s" % (link_uri, msg)) self._state = CrazyflieROS.Disconnected def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" rospy.logfatal("Connection to %s lost: %s" % (link_uri, msg)) self._state = CrazyflieROS.Disconnected def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" rospy.logfatal("Disconnected from %s" % link_uri) self._state = CrazyflieROS.Disconnected def _log_error(self, logconf, msg): """Callback from the log API when an error occurs""" rospy.logfatal("Error when logging %s: %s" % (logconf.name, msg)) def _log_data_imu(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" msg = Imu() # ToDo: it would be better to convert from timestamp to rospy time msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" msg.orientation_covariance[0] = -1 # orientation not supported # measured in deg/s; need to convert to rad/s msg.angular_velocity.x = math.radians(data["gyro.x"]) msg.angular_velocity.y = math.radians(data["gyro.y"]) msg.angular_velocity.z = math.radians(data["gyro.z"]) # measured in mG; need to convert to m/s^2 msg.linear_acceleration.x = data["acc.x"] * 9.81 msg.linear_acceleration.y = data["acc.y"] * 9.81 msg.linear_acceleration.z = data["acc.z"] * 9.81 self._pubImu.publish(msg) #print "[%d][%s]: %s" % (timestamp, logconf.name, data) def _log_data_log2(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" msg = Temperature() # ToDo: it would be better to convert from timestamp to rospy time msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in degC msg.temperature = data["baro.temp"] self._pubTemp.publish(msg) # ToDo: it would be better to convert from timestamp to rospy time msg = MagneticField() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.tf_prefix + "/base_link" # measured in Tesla msg.magnetic_field.x = data["mag.x"] msg.magnetic_field.y = data["mag.y"] msg.magnetic_field.z = data["mag.z"] self._pubMag.publish(msg) msg = Float32() # hPa (=mbar) msg.data = data["baro.pressure"] self._pubPressure.publish(msg) # V msg.data = data["pm.vbat"] self._pubBattery.publish(msg) def _param_callback(self, name, value): ros_param = "{}/{}".format(self.tf_prefix, name.replace(".", "/")) rospy.set_param(ros_param, value) def _update_params(self, req): rospy.loginfo("Update parameters %s" % (str(req.params))) for param in req.params: ros_param = "/{}/{}".format(self.tf_prefix, param) cf_param = param.replace("/", ".") print(cf_param) #if rospy.has_param(ros_param): self._cf.param.set_value(cf_param, str(rospy.get_param(ros_param))) return UpdateParamsResponse() def _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 __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) ###################################################### # By lxrocks # 'Skinny Progress Bar' tweak for Yosemite # Tweak progress bar - artistic I am not - so pick your own colors !!! # Only apply to Yosemite ###################################################### import platform if platform.system() == 'Darwin': (Version, junk, machine) = platform.mac_ver() logger.info("This is a MAC - checking if we can apply Progress " "Bar Stylesheet for Yosemite Skinny Bars ") yosemite = (10, 10, 0) tVersion = tuple(map(int, (Version.split(".")))) if tVersion >= yosemite: logger.info("Found Yosemite - applying stylesheet") tcss = """ QProgressBar { border: 1px solid grey; border-radius: 5px; text-align: center; } QProgressBar::chunk { background-color: """ + COLOR_BLUE + """; } """ self.setStyleSheet(tcss) else: logger.info("Pre-Yosemite - skinny bar stylesheet not applied") ###################################################### self.cf = Crazyflie(ro_cache=None, rw_cache=cfclient.config_path + "/cache") cflib.crtp.init_drivers( enable_debug_driver=Config().get("enable_debug_driver")) zmq_params = ZMQParamAccess(self.cf) zmq_params.start() zmq_leds = ZMQLEDDriver(self.cf) zmq_leds.start() self.scanner = ScannerThread() self.scanner.interfaceFoundSignal.connect(self.foundInterfaces) self.scanner.start() # Create and start the Input Reader self._statusbar_label = QLabel("No input-device found, insert one to" " fly.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader() self._active_device = "" # self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True) # TODO: Need to reload configs # ConfigManager().conf_needs_reload.add_callback(self._reload_configs) self.cf.connection_failed.add_callback( self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self._connection_failed) self._input_device_error_signal.connect( self._display_input_device_error) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Hide the 'File' menu on OS X, since its only item, 'Exit', gets # merged into the application menu. if sys.platform == 'darwin': self.menuFile.menuAction().setVisible(False) # Connect UI signals self.logConfigAction.triggered.connect(self._show_connect_dialog) self.interfaceCombo.currentIndexChanged['QString'].connect( self.interfaceChanged) self.connectButton.clicked.connect(self._connect) self.scanButton.clicked.connect(self._scan) self.menuItemConnect.triggered.connect(self._connect) self.menuItemConfInputDevice.triggered.connect( self._show_input_device_config_dialog) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self._update_battery) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect( self._open_config_folder) self.address.setValue(0xE7E7E7E7E7) self._auto_reconnect_enabled = Config().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect")) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self._connected) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect(self._disconnected) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self._connection_lost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect(self._connection_initiated) self._log_error_signal.connect(self._logging_error) self.batteryBar.setTextVisible(False) self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE)) self.linkQualityBar.setTextVisible(False) self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE)) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) self._selected_interface = None self._initial_scan = True self._scan() # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) self._current_input_config = None self._active_config = None self._active_config = None self.inputConfig = None # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper) self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show) self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [ dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in Config().get("open_tabs").split(","): t = tabItems[tName] if (t is not None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [{}]".format(e)) # References to all the device sub-menus in the "Input device" menu self._all_role_menus = () # Used to filter what new devices to add default mapping to self._available_devices = () # Keep track of mux nodes so we can enable according to how many # devices we have self._all_mux_nodes = () # Check which Input muxes are available self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True) for m in self.joystickReader.available_mux(): node = QAction(m.name, self._menu_inputdevice, checkable=True, enabled=False) node.toggled.connect(self._mux_selected) self._mux_group.addAction(node) self._menu_inputdevice.addAction(node) self._all_mux_nodes += (node, ) mux_subnodes = () for name in m.supported_roles(): sub_node = QMenu(" {}".format(name), self._menu_inputdevice, enabled=False) self._menu_inputdevice.addMenu(sub_node) mux_subnodes += (sub_node, ) self._all_role_menus += ({ "muxmenu": node, "rolemenu": sub_node }, ) node.setData((m, mux_subnodes)) self._mapping_support = True
class ParamExample: """ 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 """ self._cf = Crazyflie(rw_cache='./cache') # Connect some callbacks from the Crazyflie API self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) print('Connecting to %s' % link_uri) # Try to connect to the Crazyflie self._cf.open_link(link_uri) # Variable used to keep main loop occupied until disconnect self.is_connected = True self._param_check_list = [] self._param_groups = [] random.seed() 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) # Print the param TOC p_toc = self._cf.param.toc.toc for group in sorted(p_toc.keys()): print('{}'.format(group)) for param in sorted(p_toc[group].keys()): print('\t{}'.format(param)) self._param_check_list.append('{0}.{1}'.format(group, param)) self._param_groups.append('{}'.format(group)) # For every group, register the callback self._cf.param.add_update_callback(group=group, name=None, cb=self._param_callback) # You can also register a callback for a specific group.name combo self._cf.param.add_update_callback(group='cpu', name='flash', cb=self._cpu_flash_callback) print('') def _cpu_flash_callback(self, name, value): """Specific callback for the cpu.flash parameter""" print('The connected Crazyflie has {}kb of flash'.format(value)) def _param_callback(self, name, value): """Generic callback registered for all the groups""" print('{0}: {1}'.format(name, value)) # Remove each parameter from the list and close the link when # all are fetched self._param_check_list.remove(name) if len(self._param_check_list) == 0: print('Have fetched all parameter values.') # First remove all the group callbacks for g in self._param_groups: self._cf.param.remove_update_callback(group=g, cb=self._param_callback) # Create a new random value [0.00,1.00] for pid_attitude.pitch_kd # and set it pkd = random.random() print('') print('Write: pid_attitude.pitch_kd={:.2f}'.format(pkd)) self._cf.param.add_update_callback(group='pid_attitude', name='pitch_kd', cb=self._a_pitch_kd_callback) # When setting a value the parameter is automatically read back # and the registered callbacks will get the updated value self._cf.param.set_value('pid_attitude.pitch_kd', '{:.2f}'.format(pkd)) def _a_pitch_kd_callback(self, name, value): """Callback for pid_attitude.pitch_kd""" print('Readback: {0}={1}'.format(name, value)) # End the example by closing the link (will cause the app to quit) self._cf.close_link() 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)) 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 MainUI(QtGui.QMainWindow, main_window_class): connectionLostSignal = pyqtSignal(str, str) connectionInitiatedSignal = pyqtSignal(str) batteryUpdatedSignal = pyqtSignal(int, object, object) connectionDoneSignal = pyqtSignal(str) connectionFailedSignal = pyqtSignal(str, str) disconnectedSignal = pyqtSignal(str) linkQualitySignal = pyqtSignal(int) _input_device_error_signal = pyqtSignal(str) _input_discovery_signal = pyqtSignal(object) _log_error_signal = pyqtSignal(object, str) def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) ###################################################### # By lxrocks # 'Skinny Progress Bar' tweak for Yosemite # Tweak progress bar - artistic I am not - so pick your own colors !!! # Only apply to Yosemite ###################################################### import platform if platform.system() == 'Darwin': (Version, junk, machine) = platform.mac_ver() logger.info("This is a MAC - checking if we can apply Progress " "Bar Stylesheet for Yosemite Skinny Bars ") yosemite = (10, 10, 0) tVersion = tuple(map(int, (Version.split(".")))) if tVersion >= yosemite: logger.info("Found Yosemite - applying stylesheet") tcss = """ QProgressBar { border: 1px solid grey; border-radius: 5px; text-align: center; } QProgressBar::chunk { background-color: """ + COLOR_BLUE + """; } """ self.setStyleSheet(tcss) else: logger.info("Pre-Yosemite - skinny bar stylesheet not applied") ###################################################### self.cf = Crazyflie(ro_cache=None, rw_cache=cfclient.config_path + "/cache") cflib.crtp.init_drivers( enable_debug_driver=Config().get("enable_debug_driver")) zmq_params = ZMQParamAccess(self.cf) zmq_params.start() zmq_leds = ZMQLEDDriver(self.cf) zmq_leds.start() self.scanner = ScannerThread() self.scanner.interfaceFoundSignal.connect(self.foundInterfaces) self.scanner.start() # Create and start the Input Reader self._statusbar_label = QLabel("No input-device found, insert one to" " fly.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader() self._active_device = "" # self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True) # TODO: Need to reload configs # ConfigManager().conf_needs_reload.add_callback(self._reload_configs) self.cf.connection_failed.add_callback( self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self._connection_failed) self._input_device_error_signal.connect( self._display_input_device_error) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Hide the 'File' menu on OS X, since its only item, 'Exit', gets # merged into the application menu. if sys.platform == 'darwin': self.menuFile.menuAction().setVisible(False) # Connect UI signals self.logConfigAction.triggered.connect(self._show_connect_dialog) self.interfaceCombo.currentIndexChanged['QString'].connect( self.interfaceChanged) self.connectButton.clicked.connect(self._connect) self.scanButton.clicked.connect(self._scan) self.menuItemConnect.triggered.connect(self._connect) self.menuItemConfInputDevice.triggered.connect( self._show_input_device_config_dialog) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self._update_battery) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect( self._open_config_folder) self.address.setValue(0xE7E7E7E7E7) self._auto_reconnect_enabled = Config().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect")) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self._connected) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect(self._disconnected) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self._connection_lost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect(self._connection_initiated) self._log_error_signal.connect(self._logging_error) self.batteryBar.setTextVisible(False) self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE)) self.linkQualityBar.setTextVisible(False) self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE)) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) self._selected_interface = None self._initial_scan = True self._scan() # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) self._current_input_config = None self._active_config = None self._active_config = None self.inputConfig = None # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper) self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show) self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [ dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in Config().get("open_tabs").split(","): t = tabItems[tName] if (t is not None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [{}]".format(e)) # References to all the device sub-menus in the "Input device" menu self._all_role_menus = () # Used to filter what new devices to add default mapping to self._available_devices = () # Keep track of mux nodes so we can enable according to how many # devices we have self._all_mux_nodes = () # Check which Input muxes are available self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True) for m in self.joystickReader.available_mux(): node = QAction(m.name, self._menu_inputdevice, checkable=True, enabled=False) node.toggled.connect(self._mux_selected) self._mux_group.addAction(node) self._menu_inputdevice.addAction(node) self._all_mux_nodes += (node, ) mux_subnodes = () for name in m.supported_roles(): sub_node = QMenu(" {}".format(name), self._menu_inputdevice, enabled=False) self._menu_inputdevice.addMenu(sub_node) mux_subnodes += (sub_node, ) self._all_role_menus += ({ "muxmenu": node, "rolemenu": sub_node }, ) node.setData((m, mux_subnodes)) self._mapping_support = True def interfaceChanged(self, interface): if interface == INTERFACE_PROMPT_TEXT: self._selected_interface = None else: self._selected_interface = interface self._update_ui_state() def foundInterfaces(self, interfaces): selected_interface = self._selected_interface self.interfaceCombo.clear() self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT) formatted_interfaces = [] for i in interfaces: if len(i[1]) > 0: interface = "%s - %s" % (i[0], i[1]) else: interface = i[0] formatted_interfaces.append(interface) self.interfaceCombo.addItems(formatted_interfaces) if self._initial_scan: self._initial_scan = False try: selected_interface = Config().get("link_uri") except KeyError: pass newIndex = 0 if selected_interface is not None: try: newIndex = formatted_interfaces.index(selected_interface) + 1 except ValueError: pass self.interfaceCombo.setCurrentIndex(newIndex) self.uiState = UIState.DISCONNECTED self._update_ui_state() def _update_ui_state(self): if self.uiState == UIState.DISCONNECTED: self.setWindowTitle("Not connected") canConnect = self._selected_interface is not None self.menuItemConnect.setText("Connect to Crazyflie") self.menuItemConnect.setEnabled(canConnect) self.connectButton.setText("Connect") self.connectButton.setToolTip( "Connect to the Crazyflie on the selected interface") self.connectButton.setEnabled(canConnect) self.scanButton.setText("Scan") self.scanButton.setEnabled(True) self.address.setEnabled(True) self.batteryBar.setValue(3000) self._menu_cf2_config.setEnabled(False) self._menu_cf1_config.setEnabled(True) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) self.interfaceCombo.setEnabled(True) elif self.uiState == UIState.CONNECTED: s = "Connected on %s" % self._selected_interface self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.menuItemConnect.setEnabled(True) self.connectButton.setText("Disconnect") self.connectButton.setToolTip("Disconnect from the Crazyflie") self.scanButton.setEnabled(False) self.logConfigAction.setEnabled(True) # Find out if there's an I2C EEPROM, otherwise don't show the # dialog. if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0: self._menu_cf2_config.setEnabled(True) self._menu_cf1_config.setEnabled(False) elif self.uiState == UIState.CONNECTING: s = "Connecting to {} ...".format(self._selected_interface) self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.menuItemConnect.setEnabled(True) self.connectButton.setText("Cancel") self.connectButton.setToolTip("Cancel connecting to the Crazyflie") self.scanButton.setEnabled(False) self.address.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.interfaceCombo.setEnabled(False) elif self.uiState == UIState.SCANNING: self.setWindowTitle("Scanning ...") self.connectButton.setText("Connect") self.menuItemConnect.setEnabled(False) self.connectButton.setText("Connect") self.connectButton.setEnabled(False) self.scanButton.setText("Scanning...") self.scanButton.setEnabled(False) self.address.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.interfaceCombo.setEnabled(False) @pyqtSlot(bool) def toggleToolbox(self, display): menuItem = self.sender().menuItem dockToolbox = self.sender().dockToolbox if display and not dockToolbox.isVisible(): dockToolbox.widget().enable() self.addDockWidget(dockToolbox.widget().preferedDockArea(), dockToolbox) dockToolbox.show() elif not display: dockToolbox.widget().disable() self.removeDockWidget(dockToolbox) dockToolbox.hide() menuItem.setChecked(False) def _rescan_devices(self): self._statusbar_label.setText("No inputdevice connected!") self._menu_devices.clear() self._active_device = "" self.joystickReader.stop_input() # for c in self._menu_mappings.actions(): # c.setEnabled(False) # devs = self.joystickReader.available_devices() # if (len(devs) > 0): # self.device_discovery(devs) def _show_input_device_config_dialog(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked Config().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: {}".format(checked)) def _show_connect_dialog(self): self.logConfigDialogue.show() def _update_battery(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) color = COLOR_BLUE # TODO firmware reports fully-charged state as 'Battery', # rather than 'Charged' if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]: color = COLOR_GREEN elif data["pm.state"] == BatteryStates.LOW_POWER: color = COLOR_RED self.batteryBar.setStyleSheet(progressbar_stylesheet(color)) def _connected(self): self.uiState = UIState.CONNECTED self._update_ui_state() Config().set("link_uri", str(self._selected_interface)) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") lg.add_variable("pm.state", "int8_t") try: self.cf.log.add_config(lg) lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() except KeyError as e: logger.warning(str(e)) mem = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)[0] mem.write_data(self._led_write_done) # self._led_write_test = 0 # mem.leds[self._led_write_test] = [10, 20, 30] # mem.write_data(self._led_write_done) def _disconnected(self): self.uiState = UIState.DISCONNECTED self._update_ui_state() def _connection_initiated(self): self.uiState = UIState.CONNECTING self._update_ui_state() def _led_write_done(self, mem, addr): logger.info("LED write done callback") # self._led_write_test += 1 # mem.leds[self._led_write_test] = [10, 20, 30] # mem.write_data(self._led_write_done) def _logging_error(self, log_conf, msg): QMessageBox.about( self, "Log error", "Error when starting log config" " [{}]: {}".format(log_conf.name, msg)) def _connection_lost(self, linkURI, msg): if not self._auto_reconnect_enabled: if self.isActiveWindow(): warningCaption = "Communication failure" error = "Connection lost to {}: {}".format(linkURI, msg) QMessageBox.critical(self, warningCaption, error) self.uiState = UIState.DISCONNECTED self._update_ui_state() else: self._connect() def _connection_failed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on {}: {}".format(linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self.uiState = UIState.DISCONNECTED self._update_ui_state() else: self._connect() def closeEvent(self, event): self.hide() self.cf.close_link() Config().save_file() def _connect(self): if self.uiState == UIState.CONNECTED: self.cf.close_link() elif self.uiState == UIState.CONNECTING: self.cf.close_link() self.uiState = UIState.DISCONNECTED self._update_ui_state() else: self.cf.open_link(self._selected_interface) def _scan(self): self.uiState = UIState.SCANNING self._update_ui_state() self.scanner.scanSignal.emit(self.address.value()) def _display_input_device_error(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _mux_selected(self, checked): """Called when a new mux is selected. The menu item contains a reference to the raw mux object as well as to the associated device sub-nodes""" if not checked: (mux, sub_nodes) = self.sender().data() for s in sub_nodes: s.setEnabled(False) else: (mux, sub_nodes) = self.sender().data() for s in sub_nodes: s.setEnabled(True) self.joystickReader.set_mux(mux=mux) # Go though the tree and select devices/mapping that was # selected before it was disabled. for role_node in sub_nodes: for dev_node in role_node.children(): if type(dev_node) is QAction and dev_node.isChecked(): dev_node.toggled.emit(True) self._update_input_device_footer() def _get_dev_status(self, device): msg = "{}".format(device.name) if device.supports_mapping: map_name = "N/A" if device.input_map: map_name = device.input_map_name msg += " ({})".format(map_name) return msg def _update_input_device_footer(self): """Update the footer in the bottom of the UI with status for the input device and its mapping""" msg = "" if len(self.joystickReader.available_devices()) > 0: mux = self.joystickReader._selected_mux msg = "Using {} mux with ".format(mux.name) for key in list(mux._devs.keys())[:-1]: if mux._devs[key]: msg += "{}, ".format(self._get_dev_status(mux._devs[key])) else: msg += "N/A, " # Last item key = list(mux._devs.keys())[-1] if mux._devs[key]: msg += "{}".format(self._get_dev_status(mux._devs[key])) else: msg += "N/A" else: msg = "No input device found" self._statusbar_label.setText(msg) def _inputdevice_selected(self, checked): """Called when a new input device has been selected from the menu. The data in the menu object is the associated map menu (directly under the item in the menu) and the raw device""" (map_menu, device, mux_menu) = self.sender().data() if not checked: if map_menu: map_menu.setEnabled(False) # Do not close the device, since we don't know exactly # how many devices the mux can have open. When selecting a # new mux the old one will take care of this. else: if map_menu: map_menu.setEnabled(True) (mux, sub_nodes) = mux_menu.data() for role_node in sub_nodes: for dev_node in role_node.children(): if type(dev_node) is QAction and dev_node.isChecked(): if device.id == dev_node.data()[1].id \ and dev_node is not self.sender(): dev_node.setChecked(False) role_in_mux = str(self.sender().parent().title()).strip() logger.info("Role of {} is {}".format(device.name, role_in_mux)) Config().set("input_device", str(device.name)) self._mapping_support = self.joystickReader.start_input( device.name, role_in_mux) self._update_input_device_footer() def _inputconfig_selected(self, checked): """Called when a new configuration has been selected from the menu. The data in the menu object is a referance to the device QAction in parent menu. This contains a referance to the raw device.""" if not checked: return selected_mapping = str(self.sender().text()) device = self.sender().data().data()[1] self.joystickReader.set_input_map(device.name, selected_mapping) self._update_input_device_footer() def device_discovery(self, devs): """Called when new devices have been added""" for menu in self._all_role_menus: role_menu = menu["rolemenu"] mux_menu = menu["muxmenu"] dev_group = QActionGroup(role_menu, exclusive=True) for d in devs: dev_node = QAction(d.name, role_menu, checkable=True, enabled=True) role_menu.addAction(dev_node) dev_group.addAction(dev_node) dev_node.toggled.connect(self._inputdevice_selected) map_node = None if d.supports_mapping: map_node = QMenu(" Input map", role_menu, enabled=False) map_group = QActionGroup(role_menu, exclusive=True) # Connect device node to map node for easy # enabling/disabling when selection changes and device # to easily enable it dev_node.setData((map_node, d)) for c in ConfigManager().get_list_of_configs(): node = QAction(c, map_node, checkable=True, enabled=True) node.toggled.connect(self._inputconfig_selected) map_node.addAction(node) # Connect all the map nodes back to the device # action node where we can access the raw device node.setData(dev_node) map_group.addAction(node) # If this device hasn't been found before, then # select the default mapping for it. if d not in self._available_devices: last_map = Config().get("device_config_mapping") if d.name in last_map and last_map[d.name] == c: node.setChecked(True) role_menu.addMenu(map_node) dev_node.setData((map_node, d, mux_menu)) # Update the list of what devices we found # to avoid selecting default mapping for all devices when # a new one is inserted self._available_devices = () for d in devs: self._available_devices += (d, ) # Only enable MUX nodes if we have enough devies to cover # the roles for mux_node in self._all_mux_nodes: (mux, sub_nodes) = mux_node.data() if len(mux.supported_roles()) <= len(self._available_devices): mux_node.setEnabled(True) # TODO: Currently only supports selecting default mux if self._all_mux_nodes[0].isEnabled(): self._all_mux_nodes[0].setChecked(True) # If the previous length of the available devies was 0, then select # the default on. If that's not available then select the first # on in the list. # TODO: This will only work for the "Normal" mux so this will be # selected by default if Config().get("input_device") in [d.name for d in devs]: for dev_menu in self._all_role_menus[0]["rolemenu"].actions(): if dev_menu.text() == Config().get("input_device"): dev_menu.setChecked(True) else: # Select the first device in the first mux (will always be "Normal" # mux) self._all_role_menus[0]["rolemenu"].actions()[0].setChecked(True) logger.info("Select first device") self._update_input_device_footer() def _open_config_folder(self): QDesktopServices.openUrl( QUrl("file:///" + QDir.toNativeSeparators(cfclient.config_path))) def closeAppRequest(self): self.close() sys.exit(0)
class EEPROMExample: """ Simple example listing the EEPROMs found and lists its contents. """ 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) mems = self._cf.mem.get_mems(MemoryElement.TYPE_1W) print('Found {} 1-wire memories'.format(len(mems))) if len(mems) > 0: print('Writing test configuration to' ' memory {}'.format(mems[0].id)) mems[0].vid = 0xBC mems[0].pid = 0xFF board_name_id = OWElement.element_mapping[1] board_rev_id = OWElement.element_mapping[2] mems[0].elements[board_name_id] = 'Test board' mems[0].elements[board_rev_id] = 'A' mems[0].write_data(self._data_written) def _data_written(self, mem, addr): print('Data written, reading back...') mem.update(self._data_updated) def _data_updated(self, mem): print('Updated id={}'.format(mem.id)) print('\tType : {}'.format(mem.type)) print('\tSize : {}'.format(mem.size)) print('\tValid : {}'.format(mem.valid)) print('\tName : {}'.format(mem.name)) print('\tVID : 0x{:02X}'.format(mem.vid)) print('\tPID : 0x{:02X}'.format(mem.pid)) print('\tPins : 0x{:02X}'.format(mem.pins)) print('\tElements : ') for key in mem.elements: print('\t\t{}={}'.format(key, mem.elements[key])) 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('[%d][%s]: %s' % (timestamp, logconf.name, data)) def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)""" print('Connection to %s failed: %s' % (link_uri, msg)) self.is_connected = False def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print('Connection to %s lost: %s' % (link_uri, msg)) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print('Disconnected from %s' % link_uri) self.is_connected = False
def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) # Restore window size if present in the config file try: size = Config().get("window_size") self.resize(size[0], size[1]) except KeyError: pass self.cf = Crazyflie(ro_cache=None, rw_cache=cfclient.config_path + "/cache") cflib.crtp.init_drivers( enable_debug_driver=Config().get("enable_debug_driver")) zmq_params = ZMQParamAccess(self.cf) zmq_params.start() zmq_leds = ZMQLEDDriver(self.cf) zmq_leds.start() self.scanner = ScannerThread() self.scanner.interfaceFoundSignal.connect(self.foundInterfaces) self.scanner.start() # Create and start the Input Reader self._statusbar_label = QLabel("No input-device found, insert one to" " fly.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader() self._active_device = "" # self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._mux_group = QActionGroup(self._menu_inputdevice) self._mux_group.setExclusive(True) # TODO: Need to reload configs # ConfigManager().conf_needs_reload.add_callback(self._reload_configs) self.connect_input = QShortcut("Ctrl+I", self.connectButton, self._connect) self.cf.connection_failed.add_callback( self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self._connection_failed) self._input_device_error_signal.connect( self._display_input_device_error) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Hide the 'File' menu on OS X, since its only item, 'Exit', gets # merged into the application menu. if sys.platform == 'darwin': self.menuFile.menuAction().setVisible(False) # Connect UI signals self.logConfigAction.triggered.connect(self._show_connect_dialog) self.interfaceCombo.currentIndexChanged['QString'].connect( self.interfaceChanged) self.connectButton.clicked.connect(self._connect) self.scanButton.clicked.connect(self._scan) self.menuItemConnect.triggered.connect(self._connect) self.menuItemConfInputDevice.triggered.connect( self._show_input_device_config_dialog) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self._update_battery) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect( self._open_config_folder) self.address.setValue(0xE7E7E7E7E7) self._auto_reconnect_enabled = Config().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect")) self._disable_input = False self.joystickReader.input_updated.add_callback( lambda *args: self._disable_input or self.cf.commander. send_setpoint(*args)) self.joystickReader.assisted_input_updated.add_callback( lambda *args: self._disable_input or self.cf.commander. send_velocity_world_setpoint(*args)) self.joystickReader.heighthold_input_updated.add_callback( lambda *args: self._disable_input or self.cf.commander. send_zdistance_setpoint(*args)) self.joystickReader.hover_input_updated.add_callback( self.cf.commander.send_hover_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self._connected) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect(self._disconnected) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self._connection_lost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect(self._connection_initiated) self._log_error_signal.connect(self._logging_error) self.batteryBar.setTextVisible(False) self.linkQualityBar.setTextVisible(False) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) self._selected_interface = None self._initial_scan = True self._scan() # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) self._current_input_config = None self._active_config = None self._active_config = None self.inputConfig = None # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader cfclient.ui.pluginhelper.mainUI = self self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show) # Load and connect tabs self.tabsMenuItem = QMenu("Tabs", self.menuView, enabled=True) self.menuView.addMenu(self.tabsMenuItem) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) # Set reference for plot-tab. if isinstance(tab, cfclient.ui.tabs.PlotTab): cfclient.ui.pluginhelper.plotTab = tab item = QtWidgets.QAction(tab.getMenuName(), self, checkable=True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in Config().get("open_tabs").split(","): t = tabItems[tName] if (t is not None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [{}]".format(e)) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxesMenuItem = QMenu("Toolboxes", self.menuView, enabled=True) self.menuView.addMenu(self.toolboxesMenuItem) self.toolboxes = [] for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [ dockToolbox, ] # Add menu item for the toolbox item = QtWidgets.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # References to all the device sub-menus in the "Input device" menu self._all_role_menus = () # Used to filter what new devices to add default mapping to self._available_devices = () # Keep track of mux nodes so we can enable according to how many # devices we have self._all_mux_nodes = () # Check which Input muxes are available self._mux_group = QActionGroup(self._menu_inputdevice) self._mux_group.setExclusive(True) for m in self.joystickReader.available_mux(): node = QAction(m.name, self._menu_inputdevice, checkable=True, enabled=False) node.toggled.connect(self._mux_selected) self._mux_group.addAction(node) self._menu_inputdevice.addAction(node) self._all_mux_nodes += (node, ) mux_subnodes = () for name in m.supported_roles(): sub_node = QMenu(" {}".format(name), self._menu_inputdevice, enabled=False) self._menu_inputdevice.addMenu(sub_node) mux_subnodes += (sub_node, ) self._all_role_menus += ({ "muxmenu": node, "rolemenu": sub_node }, ) node.setData((m, mux_subnodes)) self._mapping_support = True # Add checkbuttons for theme-selection. self._theme_group = QActionGroup(self.menuThemes) self._theme_group.setExclusive(True) self._theme_checkboxes = [] for theme in UiUtils.THEMES: node = QAction(theme, self.menuThemes, checkable=True) node.setObjectName(theme) node.toggled.connect(self._theme_selected) self._theme_checkboxes.append(node) self._theme_group.addAction(node) self.menuThemes.addAction(node)
class MotorRampExample: """Example that connects to a Crazyflie and ramps the motors up/down and the disconnects""" def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie() self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) self._cf.open_link(link_uri) print "Connecting to %s" % link_uri def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._ramp_motors).start() def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)""" print "Connection to %s failed: %s" % (link_uri, msg) def _connection_lost(self, link_uri, msg): """Callback when disconnected after a connection has been made (i.e Crazyflie moves out of range)""" print "Connection to %s lost: %s" % (link_uri, msg) def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected (called in all cases)""" print "Disconnected from %s" % link_uri def _ramp_motors(self): thrust_mult = 1 thrust_step = 500 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) 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()
class MainUI(QtGui.QMainWindow, main_window_class): connectionLostSignal = pyqtSignal(str, str) connectionInitiatedSignal = pyqtSignal(str) batteryUpdatedSignal = pyqtSignal(int, object, object) connectionDoneSignal = pyqtSignal(str) connectionFailedSignal = pyqtSignal(str, str) disconnectedSignal = pyqtSignal(str) linkQualitySignal = pyqtSignal(int) _input_device_error_signal = pyqtSignal(str) _input_discovery_signal = pyqtSignal(object) _log_error_signal = pyqtSignal(object, str) def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") cflib.crtp.init_drivers( enable_debug_driver=GuiConfig().get("enable_debug_driver")) # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("Loading device and configuration.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader() self._active_device = "" self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._load_input_data() self._update_input ConfigManager().conf_needs_reload.add_callback(self._reload_configs) # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) self.connectionDoneSignal.connect(self.connectionDone) self.cf.connection_failed.add_callback( self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self.connectionFailed) self._input_device_error_signal.connect(self.inputDeviceError) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Connect UI signals self.menuItemConnect.triggered.connect(self.connectButtonClicked) self.logConfigAction.triggered.connect(self.doLogConfigDialogue) self.connectButton.clicked.connect(self.connectButtonClicked) self.quickConnectButton.clicked.connect(self.quickConnect) self.menuItemQuickConnect.triggered.connect(self.quickConnect) self.menuItemConfInputDevice.triggered.connect(self.configInputDevice) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self.updateBatteryVoltage) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect( self._open_config_folder) self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked( GuiConfig().get("auto_reconnect")) # Do not queue data from the controller output to the Crazyflie wrapper # to avoid latency #self.joystickReader.sendControlSetpointSignal.connect( # self.cf.commander.send_setpoint, # Qt.DirectConnection) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self.connectionDone) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect( lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI)) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self.connectionLost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect( lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI)) self._log_error_signal.connect(self._logging_error) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self.setUIState(UIState.DISCONNECTED) # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [ dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in GuiConfig().get("open_tabs").split(","): t = tabItems[tName] if (t != None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [%s]", e) def setUIState(self, newState, linkURI=""): self.uiState = newState if (newState == UIState.DISCONNECTED): self.setWindowTitle("Not connected") self.menuItemConnect.setText("Connect to Crazyflie") self.connectButton.setText("Connect") self.menuItemQuickConnect.setEnabled(True) self.batteryBar.setValue(3000) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) if (len(GuiConfig().get("link_uri")) > 0): self.quickConnectButton.setEnabled(True) if (newState == UIState.CONNECTED): s = "Connected on %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.connectButton.setText("Disconnect") self.logConfigAction.setEnabled(True) if (newState == UIState.CONNECTING): s = "Connecting to %s ..." % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.connectButton.setText("Cancel") self.quickConnectButton.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.menuItemQuickConnect.setEnabled(False) @pyqtSlot(bool) def toggleToolbox(self, display): menuItem = self.sender().menuItem dockToolbox = self.sender().dockToolbox if display and not dockToolbox.isVisible(): dockToolbox.widget().enable() self.addDockWidget(dockToolbox.widget().preferedDockArea(), dockToolbox) dockToolbox.show() elif not display: dockToolbox.widget().disable() self.removeDockWidget(dockToolbox) dockToolbox.hide() menuItem.setChecked(False) def _rescan_devices(self): self._statusbar_label.setText("No inputdevice connected!") self._menu_devices.clear() self._active_device = "" self.joystickReader.stop_input() for c in self._menu_mappings.actions(): c.setEnabled(False) devs = self.joystickReader.getAvailableDevices() if (len(devs) > 0): self.device_discovery(devs) def configInputDevice(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked GuiConfig().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: %s", checked) def doLogConfigDialogue(self): self.logConfigDialogue.show() def updateBatteryVoltage(self, timestamp, data, logconf): batteryVoltage = int(data["pm.vbat"] * 1000) #self.joystickReader.setEmergencyLanding(batteryVoltage < 3000 and batteryVoltage-self.batteryBar.getValue() < 0) self.batteryBar.setValue(batteryVoltage) def connectionDone(self, linkURI): self.setUIState(UIState.CONNECTED, linkURI) GuiConfig().set("link_uri", linkURI) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") self.cf.log.add_config(lg) if lg.valid: lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() else: logger.warning("Could not setup loggingblock!") def _logging_error(self, log_conf, msg): QMessageBox.about( self, "Log error", "Error when starting log config" " [%s]: %s" % (log_conf.name, msg)) def connectionLost(self, linkURI, msg): if not self._auto_reconnect_enabled: if (self.isActiveWindow()): warningCaption = "Communication failure" error = "Connection lost to %s: %s" % (linkURI, msg) QMessageBox.critical(self, warningCaption, error) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def connectionFailed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on %s: %s" % (linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def closeEvent(self, event): self.hide() self.cf.close_link() GuiConfig().save_file() def connectButtonClicked(self): if (self.uiState == UIState.CONNECTED): self.cf.close_link() elif (self.uiState == UIState.CONNECTING): self.cf.close_link() self.setUIState(UIState.DISCONNECTED) else: self.connectDialogue.show() def inputDeviceError(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _load_input_data(self): self.joystickReader.stop_input() # Populate combo box with available input device configurations for c in ConfigManager().get_list_of_configs(): node = QAction(c, self._menu_mappings, checkable=True, enabled=False) node.toggled.connect(self._inputconfig_selected) self.configGroup.addAction(node) self._menu_mappings.addAction(node) def _reload_configs(self, newConfigName): # remove the old actions from the group and the menu for action in self._menu_mappings.actions(): self.configGroup.removeAction(action) self._menu_mappings.clear() # reload the conf files, and populate the menu self._load_input_data() self._update_input(self._active_device, newConfigName) def _update_input(self, device="", config=""): self.joystickReader.stop_input() self._active_config = str(config) self._active_device = str(device) GuiConfig().set("input_device", self._active_device) GuiConfig().get("device_config_mapping")[ self._active_device] = self._active_config self.joystickReader.start_input(self._active_device, self._active_config) # update the checked state of the menu items for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._active_config: c.setChecked(True) for c in self._menu_devices.actions(): c.setEnabled(True) if c.text() == self._active_device: c.setChecked(True) # update label if device == "" and config == "": self._statusbar_label.setText("No input device selected") elif config == "": self._statusbar_label.setText("Using [%s] - " "No input config selected" % (self._active_device)) else: self._statusbar_label.setText( "Using [%s] with config [%s]" % (self._active_device, self._active_config)) def _inputdevice_selected(self, checked): if (not checked): return self.joystickReader.stop_input() sender = self.sender() self._active_device = sender.text() device_config_mapping = GuiConfig().get("device_config_mapping") if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[str( self._active_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text( ) GuiConfig().set("input_device", str(self._active_device)) for c in self._menu_mappings.actions(): if (c.text() == self._current_input_config): c.setChecked(True) self.joystickReader.start_input(str(sender.text()), self._current_input_config) self._statusbar_label.setText( "Using [%s] with config [%s]" % (self._active_device, self._current_input_config)) def _inputconfig_selected(self, checked): if (not checked): return self._update_input(self._active_device, self.sender().text()) def device_discovery(self, devs): group = QActionGroup(self._menu_devices, exclusive=True) for d in devs: node = QAction(d["name"], self._menu_devices, checkable=True) node.toggled.connect(self._inputdevice_selected) group.addAction(node) self._menu_devices.addAction(node) if (d["name"] == GuiConfig().get("input_device")): self._active_device = d["name"] if (len(self._active_device) == 0): self._active_device = self._menu_devices.actions()[0].text() device_config_mapping = GuiConfig().get("device_config_mapping") if (device_config_mapping): if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[str( self._active_device)] else: self._current_input_config = self._menu_mappings.actions( )[0].text() else: self._current_input_config = self._menu_mappings.actions()[0].text( ) # Now we know what device to use and what mapping, trigger the events # to change the menus and start the input for c in self._menu_mappings.actions(): c.setEnabled(True) if (c.text() == self._current_input_config): c.setChecked(True) for c in self._menu_devices.actions(): if (c.text() == self._active_device): c.setChecked(True) def quickConnect(self): try: self.cf.open_link(GuiConfig().get("link_uri")) except KeyError: self.cf.open_link("") def _open_config_folder(self): QDesktopServices.openUrl( QUrl("file:///" + QDir.toNativeSeparators(sys.path[1]))) def closeAppRequest(self): self.close() sys.exit(0)
class Crazyflie: # ID is for human readability def __init__(self, cf_id, radio_uri, data_only=False): self._id = cf_id self._uri = radio_uri self.stop_sig = False signal.signal(signal.SIGINT, self.signal_handler) self.cf_active = False self.accept_commands = False self.data_only = data_only self.data = None self.alt = 0 # self.bridge = CvBridge() cflib.crtp.init_drivers(enable_debug_driver=False) # try: # with SyncCrazyflie(self._uri) as scf: self.cf = CF(rw_cache="./cache") self.cf.connected.add_callback(self.connected) self.cf.disconnected.add_callback(self.disconnected) self.cf.connection_failed.add_callback(self.connection_lost) self.cf.connection_lost.add_callback(self.connection_failed) print('Connecting to %s' % radio_uri) self.cf.open_link(radio_uri) # self.cf.param.set_value('kalman.resetEstimation', '1') # time.sleep(0.1) # self.cf.param.set_value('kalman.resetEstimation', '0') # time.sleep(1.5) # except Exception as e: # print(type(e)) # print("Unable to connect to CF %d at URI %s" % (self._id, self._uri)) # self.scf = None # self.cf = None self.data_pub = rospy.Publisher('cf/%d/data' % self._id, CFData, queue_size=10) # self.image_pub = rospy.Publisher('cf/%d/image'%self._id, Image, queue_size=10) if not self.data_only: self.cmd_sub = rospy.Subscriber('cf/%d/command' % self._id, CFCommand, self.command_cb) self.motion_sub = rospy.Subscriber('cf/%d/motion' % self._id, CFMotion, self.motion_cb) def signal_handler(self, sig, frame): if self.cf_active: self.cmd_estop() self.stop_sig = True rospy.signal_shutdown("CtrlC") #killing os.kill(os.getpgrp(), signal.SIGKILL) ## CALLBACKS ## def connected(self, uri): print("Connected to Crazyflie at URI: %s" % uri) self.cf_active = True try: self.log_data = LogConfig(name="Data", period_in_ms=10) self.log_data.add_variable('acc.x', 'float') self.log_data.add_variable('acc.y', 'float') self.log_data.add_variable('acc.z', 'float') self.log_data.add_variable('pm.vbat', 'float') self.log_data.add_variable('stateEstimate.z', 'float') self.cf.log.add_config(self.log_data) self.log_data.data_received_cb.add_callback(self.received_data) #begins logging and publishing self.log_data.start() print("Logging Setup Complete. Starting...") except KeyError as e: print('Could not start log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add log config, bad configuration.') def disconnected(self, uri): self.cf_active = False print("Disconnected from Crazyflie at URI: %s" % uri) def connection_failed(self, uri, msg): self.cf_active = False print("Connection Failed") def connection_lost(self, uri, msg): self.cf_active = False print("Connection Lost") def command_cb(self, msg): print("ALT: %.3f" % self.alt) if self.accept_commands: print("RECEIVED COMMAND : %s" % cmd_type[msg.cmd]) if cmd_type[msg.cmd] == 'ESTOP': self.cmd_estop() elif cmd_type[msg.cmd] == 'LAND': self.alt = 0 self.cmd_land() elif cmd_type[msg.cmd] == 'TAKEOFF': self.alt = 0.4 self.cmd_takeoff() else: print('Invalid Command! %d' % msg.cmd) elif cmd_type[msg.cmd] == 'TAKEOFF': self.alt = 0.4 self.cmd_takeoff() else: print("Not Accepting Commands -- but one was sent!") def motion_cb(self, msg): print("ALT: %.3f" % self.alt) print(msg) if self.accept_commands: self.update_alt(msg) # switching between optical flow and roll pitch motion if msg.is_flow_motion: self.set_flow_motion(msg.x, msg.y, msg.yaw, self.alt) else: self.set_rp_motion(msg.x, msg.y, msg.yaw, self.alt) else: print("Not Accepting Motion Commands -- but one was sent!") def update_alt(self, msg): #what exactly does this do? #motion.alt = self.data.alt * 100 if self.data.alt > ALT_TOLERANCE else 0 self.alt += msg.dz if self.alt < 0: self.alt = 0 elif self.alt > MAX_ALT: self.alt = MAX_ALT def received_data(self, timestamp, data, logconf): # print("DATA RECEIVED") # print(self.data) self.data = data d = CFData() d.ID = self._id d.accel_x = float(data['acc.x']) d.accel_y = float(data['acc.y']) d.accel_z = float(data['acc.z']) d.v_batt = float(data['pm.vbat']) d.alt = float(data['stateEstimate.z']) # d.alt = float(data['posEstimatorAlt.estimatedZ']) # print(d.v_batt) self.data_pub.publish(d) ## COMMANDS ## def set_flow_motion(self, vx, vy, yaw, alt): self.cf.commander.send_hover_setpoint(vx, vy, yaw, alt) def set_rp_motion(self, roll_a, pitch_a, yaw_r, alt): self.cf.commander.send_zdistance_setpoint(roll_a, pitch_a, yaw_r, alt) def cmd_estop(self): print("---- Crazyflie %d Emergency Stopping ----" % self._id) self.cf.commander.send_stop_setpoint() self.accept_commands = False def cmd_takeoff(self, alt=0.4): for y in range(10): print("taking off") self.cf.commander.send_hover_setpoint(0, 0, 0, y / 10 * alt) time.sleep(0.1) self.accept_commands = True def cmd_land(self, alt=0.4): if self.accept_commands == False: print("cannot land right now") else: for y in range(10): self.cf.commander.send_hover_setpoint(0, 0, 0, alt - (y / 10 * alt)) time.sleep(0.1) self.cmd_estop() def run(self): print("WAITING FOR ACTIVE CONNECTION") while not self.cf_active: pass print("FOUND ACTIVE CONNECTION") #handles image reads # threading.Thread(target=self.image_thread).start() rate = rospy.Rate(25) rospy.spin() self.log_data.stop()
def __init__(self, *args): super(MainUI, self).__init__(*args) self.setupUi(self) self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") cflib.crtp.init_drivers( enable_debug_driver=GuiConfig().get("enable_debug_driver")) # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("Loading device and configuration.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader() self._active_device = "" self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._load_input_data() self._update_input ConfigManager().conf_needs_reload.add_callback(self._reload_configs) # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) self.connectionDoneSignal.connect(self.connectionDone) self.cf.connection_failed.add_callback( self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self.connectionFailed) self._input_device_error_signal.connect(self.inputDeviceError) self.joystickReader.device_error.add_callback( self._input_device_error_signal.emit) self._input_discovery_signal.connect(self.device_discovery) self.joystickReader.device_discovery.add_callback( self._input_discovery_signal.emit) # Connect UI signals self.menuItemConnect.triggered.connect(self.connectButtonClicked) self.logConfigAction.triggered.connect(self.doLogConfigDialogue) self.connectButton.clicked.connect(self.connectButtonClicked) self.quickConnectButton.clicked.connect(self.quickConnect) self.menuItemQuickConnect.triggered.connect(self.quickConnect) self.menuItemConfInputDevice.triggered.connect(self.configInputDevice) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self.updateBatteryVoltage) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect( self._open_config_folder) self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked( GuiConfig().get("auto_reconnect")) # Do not queue data from the controller output to the Crazyflie wrapper # to avoid latency #self.joystickReader.sendControlSetpointSignal.connect( # self.cf.commander.send_setpoint, # Qt.DirectConnection) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self.connectionDone) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect( lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI)) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self.connectionLost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect( lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI)) self._log_error_signal.connect(self._logging_error) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self.setUIState(UIState.DISCONNECTED) # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [ dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().addAction(item) tabItems[tab.getTabName()] = item self.loadedTabs.append(tab) if not tab.enabled: item.setEnabled(False) # First instantiate all tabs and then open them in the correct order try: for tName in GuiConfig().get("open_tabs").split(","): t = tabItems[tName] if (t != None and t.isEnabled()): # Toggle though menu so it's also marked as open there t.toggle() except Exception as e: logger.warning("Exception while opening tabs [%s]", e)
class ESA_Server(threading.Thread): """ Simple logging example class that logs the Stabilizer, Zrange, and sends commands for the supplied link uri. """ def __init__(self, link_uri): """ Initialize and run the example with the specified link_uri """ super(ESA_Server, self).__init__() self.daemon = True self.link_uri = link_uri self.sensorsData = [] # 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) self.is_connected = False self._socket = socket self.is_ready = False self.stop_print = False logging.log(logging.INFO, ('Initialized the %s' % self.link_uri)) def run(self): logging.log(logging.INFO, "Starting " + self.link_uri) logging.log( logging.INFO, "Thread name : " + threading.current_thread().name + " is starting with drone URI: " + self.link_uri) while self.is_connected: continue def cancel(self): """End this thread""" self.is_connected = False 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.is_connected = True self._lg_sensorData = LogConfig(name='Stabilizer', period_in_ms=60) self._lg_sensorData.add_variable('stabilizer.roll', 'float') self._lg_sensorData.add_variable('stabilizer.pitch', 'float') self._lg_sensorData.add_variable('stabilizer.yaw', 'float') self._lg_sensorData.add_variable('range.zrange', 'float') try: self._cf.log.add_config( self._lg_sensorData) # This callback will receive the data self._lg_sensorData.data_received_cb.add_callback( self._sensorData_log_data ) # This callback will be called on errors self._lg_sensorData.error_cb.add_callback( self._sensorData_log_error) # Start the logging self._lg_sensorData.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 unlock_thrust_protection(self): # Unlock startup thrust protection self._cf.commander.send_setpoint(0, 0, 0, 0) def send_zrange_setpoint(self, roll=0, pitch=0, yawrate=0, zdistance=0): self._cf.commander.send_zdistance_setpoint(roll, pitch, yawrate, zdistance) def send_setpoint(self, roll=0, pitch=0, yawrate=0, thrust=0): self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) def send_hover_setpoint(self, vx=0, vy=0, yawrate=0, zdistance=0): self._cf.commander.send_hover_setpoint(vx, vy, yawrate, zdistance) def _sensorData_log_error(self, logconf, msg): """Callback from the log API when an error occurs""" print('Error when logging %s: %s' % (logconf.name, msg)) def _sensorData_log_data(self, timestamp, data, logconf): """Callback froma the log API when data arrives""" logging.log(logging.INFO, '[%d][%s]: %s' % (timestamp, self.link_uri, data)) data['uri'] = self.link_uri drone = {} drone['drone'] = data self.sensorsData = drone if not self.stop_print: print('[%d][%s]: %s' % (timestamp, logconf.name, self.sensorsData)) self.is_ready = True self.stop_print = True 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 altHold(self): ''' stays at the current height :return: ''' self._cf.param.set_value("flightmode.althold", "1")
with open('../../Sequences/Straight Line.json') as jsonFile: seq_data = json.load(jsonFile) color_data = seq_data['Tracks'][0]['LedTimings'] def get_color(r, g, b): return (int(r) << 16) | (int(g) << 8) | int(b) def write_finished(*args): print("Args: ", args) print("\n\n------- WRITE FINISHED --------\n\n") os.environ["USE_CFLINK"] = "cpp" cflib.crtp.init_drivers() with SyncCrazyflie("radio://*/55/2M/E7E7E7E701?safelink=1&autoping=1", cf=Crazyflie(ro_cache='../cache', rw_cache='../cache')) as scf: cf = scf.cf time.sleep(0.5) cf.light_controller.set_effect(RingEffect.OFF) time.sleep(0.5) fadeTime = 0.25 sleepTime = 0.5 cf.light_controller.set_effect(RingEffect.FADE_EFFECT) cf.light_controller.set_color(255, 0, 0, fadeTime) time.sleep(sleepTime) cf.light_controller.set_color(0, 0, 0, fadeTime) time.sleep(sleepTime) cf.light_controller.set_color(0, 255, 0, fadeTime)
def _flash_deck(self, artifacts: List[FlashArtifact], targets: List[Target]): flash_all_targets = len(targets) == 0 if self.progress_cb: self.progress_cb('Detecting deck to be updated', int(25)) with SyncCrazyflie(self.clink, cf=Crazyflie()) as scf: deck_mems = scf.cf.mem.get_mems(MemoryElement.TYPE_DECK_MEMORY) deck_mems_count = len(deck_mems) if deck_mems_count == 0: return mgr = deck_memory.SyncDeckMemoryManager(deck_mems[0]) decks = mgr.query_decks() for (deck_index, deck) in decks.items(): if self.terminate_flashing_cb and self.terminate_flashing_cb(): raise Exception('Flashing terminated') # Check that we want to flash this deck deck_target = [ t for t in targets if t == Target('deck', deck.name, 'fw') ] if (not flash_all_targets) and len(deck_target) == 0: print(f'Skipping {deck.name}') continue # Check that we have an artifact for this deck deck_artifacts = [ a for a in artifacts if a.target == Target('deck', deck.name, 'fw') ] if len(deck_artifacts) == 0: print( f'Skipping {deck.name}, no artifact for it in the .zip' ) continue deck_artifact = deck_artifacts[0] if self.progress_cb: self.progress_cb(f'Updating deck {deck.name}', int(50)) print(f'Handling {deck.name}') # Test and wait for the deck to be started while not deck.is_started: print('Deck not yet started ...') time.sleep(500) deck = mgr.query_decks()[deck_index] # Run a brunch of sanity checks ... if not deck.supports_fw_upgrade: print( f'Deck {deck.name} does not support firmware update, skipping!' ) continue if not deck.is_fw_upgrade_required: print(f'Deck {deck.name} firmware up to date, skipping') continue if not deck.is_bootloader_active: print( f'Error: Deck {deck.name} bootloader not active, skipping!' ) continue # ToDo, white the correct file there ... result = deck.write_sync(0, deck_artifact.content) if result: if self.progress_cb: self.progress_cb( f'Deck {deck.name} updated succesfully!', int(75)) else: if self.progress_cb: self.progress_cb(f'Failed to update deck {deck.name}', int(0)) raise Exception(f'Failed to update deck {deck.name}')
class HealthTest: """ Represents the Crazyflie and handles its connection to the radio. The callbacks are then passed to the main gui object which handles the communication between the visual graphics. """ def __init__(self, uri, main_gui, test): self.cf = Crazyflie(rw_cache='./cache') self.uri = uri self.main_gui = main_gui self.link_is_open = False self.is_hover_test_running = False self.motor_means = [0, 0, 0, 0] self.motor_mean_counter = 0 self._connect = Event() self._logconfig = Event() self.test = test self.variance = {} def open_link(self): """ Adds callbacks then tries to connect to the Crazyflie. If no response, send an error msg to main_gui (GUI) and remove callbacks, else, add log configurations. """ if self.link_is_open: self.main_gui.warning_msg('Link is already open') else: self.add_callbacks() self.main_gui.connecting(self.uri) self.cf.open_link(self.uri) self._connect.wait() if not self.link_is_open: self.remove_callbacks() self.main_gui.warning_msg("Couldn't open link") else: self.add_logconfigs() self._logconfig.wait() return self def run_test(self): if self.test == 'propeller': self.propeller_test() else: self.hover_test() def hover_test(self): """ Takes off and hovers at x m for y seconds. Default is 0.5m and 5 seconds. A mean motor thrust for each motor is taken and passed to the main gui for calculations. """ height = 0.5 duration = 5 self.main_gui.running_test(self.uri) MotionCommander.VELOCITY = 0.8 with MotionCommander(self.cf, height) as mc: self.is_hover_test_running = True mc.stop() time.sleep(duration) self.hover_test_done() def propeller_test(self): self.main_gui.running_test(self.uri) self.cf.param.set_value('health.startPropTest', '0') time.sleep(0.2) self.initial_counter = self.motor_pass_counter self.cf.param.set_value('health.startPropTest', "1") time.sleep(5) while self.initial_counter == self.motor_pass_counter: time.sleep(0.1) self.propeller_test_done() def hover_test_done(self): try: means = [(total / self.motor_mean_counter) for total in self.motor_means] finally: self.main_gui.hover_test_done(self.uri, means) def propeller_test_done(self): """ Sends the results to the main gui object """ self.main_gui.propeller_test_done(self.motorlog, self.uri) def add_callbacks(self): 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) def remove_callbacks(self): try: self.cf.connected.remove_callback(self.connected) self.cf.connection_lost.remove_callback(self.connection_lost) self.cf.connection_failed.remove_callback(self.connection_failed) self.cf.disconnected.remove_callback(self.disconnected) except ValueError: pass def close_link(self): self.cf.close_link() self.remove_callbacks() self.link_is_open = False def add_logconfigs(self): """ The TOC's are motor thrust, battery level and results from propeller test """ self._log = LogConfig(self.uri, period_in_ms=50) self._log.add_variable("pwm.m1_pwm", "uint32_t") self._log.add_variable("pwm.m2_pwm", "uint32_t") self._log.add_variable("pwm.m3_pwm", "uint32_t") self._log.add_variable("pwm.m4_pwm", "uint32_t") self._log.add_variable("pm.vbatMV", "uint16_t") self._log.add_variable('health.motorTestCount', 'uint16_t') self._log.add_variable('health.motorPass', 'uint8_t') self._log.data_received_cb.add_callback(self.log_callback) self.cf.log.add_config(self._log) self._log.start() self._logconfig.set() def log_callback(self, timestamp, data, logconf): m1, m2 = data['pwm.m1_pwm'], data['pwm.m2_pwm'] m3, m4 = data['pwm.m3_pwm'], data['pwm.m4_pwm'] motor_values = [m1, m2, m3, m4] battery = data['pm.vbatMV'] self.motorlog = data['health.motorPass'] self.motor_pass_counter = data['health.motorTestCount'] if self.is_hover_test_running: for i, data in enumerate(motor_values): self.motor_means[i] += data self.motor_mean_counter += 1 self.main_gui.cb_logs(self.uri, motor_values, battery) # Connection callbacks from Crazyflie def connected(self, callback): self.link_is_open = True self._connect.set() self.main_gui.connected(self.uri) def disconnected(self, *args): self.main_gui.disconnected(self.uri) def connection_failed(self, *args): self.main_gui.connection_failed(self.uri) def connection_lost(self, *args): self.main_gui.connection_lost(self.uri) def __enter__(self): """ In case of use with wrapper """ self.open_link() def __exit__(self, *args): """ In case of use with wrapper """ self.close_link()
class Drone: """Represents a CrazyFlie drone.""" def __init__(self, drone_id: str, radio_id: int = 0, channel: int = 80, address: str = "E7E7E7E7E7", data_rate: str = "2M"): """ Initializes the drone with the given uri.""" # Initialize public variables self.id: str = drone_id self.var_x: float = 0 self.var_y: float = 0 self.var_z: float = 0 self.pos_x: float = 0 self.pos_y: float = 0 self.pos_z: float = 0 self.yaw: float = 0 self.battery_voltage: float = 0 self.is_connected: bool = False self.status: DroneState = DroneState.OFFLINE self.link_uri: str = "radio://" + str(radio_id) + "/" + str( channel) + "/" + data_rate + "/" + address # Initialize limits self._max_velocity: float = 0.2 self._min_duration: float = 1 self._max_yaw_rotations: float = 1 self._arena: Arena = Arena() # Event to asynchronously wait for the connection self._connect_event = threading.Event() # Initialize the crazyflie self._cf = Crazyflie(rw_cache='./cache') # Initialize the callbacks 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) # Initialize events self.drone_lost = Caller() # Define the log configuration self._log_config_1 = LogConfig(name='DroneLog_1', period_in_ms=500) self._log_config_1.add_variable('kalman.varPX', 'float') self._log_config_1.add_variable('kalman.varPY', 'float') self._log_config_1.add_variable('kalman.varPZ', 'float') self._log_config_1.add_variable('pm.vbat', 'float') self._log_config_2 = LogConfig(name='DroneLog_2', period_in_ms=500) self._log_config_2.add_variable('kalman.stateX', 'float') self._log_config_2.add_variable('kalman.stateY', 'float') self._log_config_2.add_variable('kalman.stateZ', 'float') self._log_config_2.add_variable('stabilizer.yaw', 'float') def connect(self, synchronous: bool = False): """Connects to the Crazyflie.""" self._connect_crazyflie() if synchronous: self._connect_event.wait() def disconnect(self): """Disconnects from the Crazyflie and stops all logging.""" self._disconnect_crazyflie() def enable_high_level_commander(self): """Enables the drones high level commander.""" self._cf.param.set_value('commander.enHighLevel', '1') time.sleep(0.1) def disable_motion_tracking(self): """Disables to motion control (x/y) from the flow-deck.""" self._cf.param.set_value('motion.disable', '1') time.sleep(0.1) def get_status(self) -> str: """Gets various information of the drone.""" return { "id": self.id, "var_x": self.var_x, "var_y": self.var_y, "var_z": self.var_z, "x": self.pos_x, "y": self.pos_y, "z": self.pos_z, "yaw": self.yaw, "status": self.status.name, "battery_voltage": self.battery_voltage, "battery_percentage:": (self.battery_voltage - 3.4) / (4.18 - 3.4) * 100 } def reset_estimator(self) -> bool: """Resets the position estimates.""" self._cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2.0) # TODO: wait_for_position_estimator(cf) return True def takeoff(self, absolute_height: float, velocity: float, synchronous: bool = False) -> float: absolute_height = self._sanitize_z(absolute_height, False) self.reset_estimator() duration = self._convert_velocity_to_time(absolute_height, velocity) self._cf.high_level_commander.takeoff(absolute_height, duration) self.status = DroneState.STARTING if synchronous: time.sleep(duration) return {"duration": duration, "target_z": absolute_height} def land(self, absolute_height: float, velocity: float, synchronous: bool = False) -> float: absolute_height = self._sanitize_z(absolute_height, False) duration = self._convert_velocity_to_time(absolute_height, velocity) self._cf.high_level_commander.land(absolute_height, duration) self.status = DroneState.LANDING if synchronous: time.sleep(duration) return {"duration": duration, "target_z": absolute_height} def go_to(self, x: float, y: float, z: float, yaw: float, velocity: float, relative: bool = False, synchronous: bool = False) -> float: x = self._sanitize_x(x, relative) y = self._sanitize_y(y, relative) z = self._sanitize_z(z, relative) yaw = self._sanitize_yaw(yaw) distance = self._calculate_distance(x, y, z, relative) duration = self._convert_velocity_to_time(distance, velocity) self._cf.high_level_commander.go_to(x, y, z, yaw, duration, relative) self.status = DroneState.NAVIGATING if synchronous: time.sleep(duration) return { "duration": duration, "target_x": x, "target_y": y, "target_z": z, "target_yaw": yaw, "relative": relative } def stop(self): self._cf.high_level_commander.stop() self.status = DroneState.IDLE def _connect_crazyflie(self): print('Connecting to %s' % self.link_uri) self._cf.open_link(self.link_uri) def _disconnect_crazyflie(self): print('Disconnecting from %s' % self.link_uri) # Stop the loggers self._log_config_1.stop() self._log_config_2.stop() # Shutdown the rotors self._shutdown() # Disconnect self._cf.close_link() def _connected(self, link_uri): """This callback is called when the Crazyflie has been connected and the TOCs have been downloaded.""" print('Connected to %s' % link_uri) # Setup parameters self.disable_motion_tracking() # Add the logger self._cf.log.add_config(self._log_config_1) self._cf.log.add_config(self._log_config_2) # This callback will receive the data self._log_config_1.data_received_cb.add_callback( self._log_config_1_data) self._log_config_2.data_received_cb.add_callback( self._log_config_2_data) # This callback will be called on errors self._log_config_1.error_cb.add_callback(self._log_config_error) self._log_config_2.error_cb.add_callback(self._log_config_error) # Start the logging self._log_config_1.start() self._log_config_2.start() # Set the connected event self._connect_event.set() self.is_connected = True self.status = DroneState.IDLE def _connection_failed(self, link_uri, msg): """Callback when the initial connection fails.""" print('Connection to %s failed: %s' % (link_uri, msg)) # Set the connected event self._connect_event.set() def _disconnected(self, link_uri): """Callback when the Crazyflie is disconnected.""" print('Disconnected from %s' % link_uri) self.is_connected = False self.status = DroneState.OFFLINE def _connection_lost(self, link_uri, msg): """Callback when the connection is lost after a connection has been made.""" print('Connection to %s lost: %s' % (link_uri, msg)) self.drone_lost.call(self) self._connect_event.set() self.is_connected = False self.status = DroneState.OFFLINE def _log_config_error(self, logconf, msg): """Callback from the log API when an error occurs.""" print('Error when logging %s: %s' % (logconf.name, msg)) def _log_config_1_data(self, timestamp, data, logconf): """Callback from the log API when data arrives.""" self.var_x = data['kalman.varPX'] self.var_y = data['kalman.varPY'] self.var_z = data['kalman.varPZ'] self.battery_voltage = data['pm.vbat'] def _log_config_2_data(self, timestamp, data, logconf): """Callback from the log API when data arrives.""" self.pos_x = data['kalman.stateX'] self.pos_y = data['kalman.stateY'] self.pos_z = data['kalman.stateZ'] self.yaw = data['stabilizer.yaw'] def _unlock(self): # Unlock startup thrust protection (only needed for low lewel commands) self._cf.commander.send_setpoint(0, 0, 0, 0) def _shutdown(self): 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) def _keep_setpoint(self, roll, pitch, yawrate, thrust, keeptime): """Keeps the drone at the given setpoint for the given amount of time.""" while keeptime > 0: self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust) keeptime -= 0.1 time.sleep(0.1) def _convert_velocity_to_time(self, distance: float, velocity: float) -> float: """Converts a distance and a velocity to a time.""" duration = distance / self._sanitize_velocity(velocity) return self._sanitize_duration(duration) def _calculate_distance(self, x: float, y: float, z: float, relative: bool = False) -> float: """Calculates the distance from the drone or the zero position (relative) to a given point in space.""" start_x = 0 if relative else self.pos_x start_y = 0 if relative else self.pos_y start_z = 0 if relative else self.pos_z return np.sqrt((x - start_x)**2 + (y - start_y)**2 + (z - start_z)**2) def _sanitize_velocity(self, velocity: float) -> float: return min(velocity, self._max_velocity) def _sanitize_duration(self, duration: float) -> float: return max(duration, self._min_duration) def _sanitize_yaw(self, yaw: float) -> float: return yaw % (2 * self._max_yaw_rotations * math.pi) def _sanitize_x(self, x: float, relative: bool) -> float: target_x = (self.pos_x + x) if relative else x return self._sanitize_number(target_x, self._arena.min_x, self._arena.max_x) def _sanitize_y(self, y: float, relative: bool) -> float: target_y = (self.pos_y + y) if relative else y return self._sanitize_number(target_y, self._arena.min_y, self._arena.max_y) def _sanitize_z(self, z: float, relative: bool) -> float: target_z = (self.pos_z + z) if relative else z return self._sanitize_number(target_z, self._arena.min_z, self._arena.max_z) def _sanitize_number(self, value: float, min_value: float, max_value: float) -> float: return min(max(value, min_value), max_value)
# Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) if __name__ == '__main__': # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) print('Scanning interfaces for Crazyflies...') available = cflib.crtp.scan_interfaces() if available: print('Drones found:') for i in available: print(i[0]) with SyncCrazyflie(available[0][0], cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf print('resetting estimator') cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) print('sending commands...') for _ in range(50): cf.commander.send_hover_setpoint(0, 0, 0, 1) time.sleep(0.1) alt = 1 steps = 30
class SyncCrazyflie: def __init__(self, link_uri, cf=None): """ Create a synchronous Crazyflie instance with the specified link_uri """ if cf: self.cf = cf else: self.cf = Crazyflie() self._link_uri = link_uri self._connect_event = None self._disconnect_event = None self._is_link_open = False self._error_message = None def open_link(self): if (self.is_link_open()): raise Exception('Link already open') self._add_callbacks() logger.debug('Connecting to %s' % self._link_uri) self._connect_event = Event() self.cf.open_link(self._link_uri) self._connect_event.wait() self._connect_event = None if not self._is_link_open: self._remove_callbacks() raise Exception(self._error_message) def __enter__(self): self.open_link() return self def close_link(self): if (self.is_link_open()): self._disconnect_event = Event() self.cf.close_link() self._disconnect_event.wait() self._disconnect_event = None def __exit__(self, exc_type, exc_val, exc_tb): self.close_link() def is_link_open(self): return self._is_link_open 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.""" logger.debug('Connected to %s' % link_uri) self._is_link_open = True if self._connect_event: self._connect_event.set() def _connection_failed(self, link_uri, msg): """Callback when initial connection fails (i.e no Crazyflie at the specified address)""" logger.debug('Connection to %s failed: %s' % (link_uri, msg)) self._is_link_open = False self._error_message = msg if self._connect_event: self._connect_event.set() def _disconnected(self, link_uri): self._remove_callbacks() self._is_link_open = False if self._disconnect_event: self._disconnect_event.set() def _add_callbacks(self): self.cf.connected.add_callback(self._connected) self.cf.connection_failed.add_callback(self._connection_failed) self.cf.disconnected.add_callback(self._disconnected) def _remove_callbacks(self): def remove_callback(container, callback): try: container.remove_callback(callback) except ValueError: pass remove_callback(self.cf.connected, self._connected) remove_callback(self.cf.connection_failed, self._connection_failed) remove_callback(self.cf.disconnected, self._disconnected)
def complex_flight(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: with PositionHlCommander( scf, x=1.0, y=1.0, z=0.0, default_velocity=0.3, default_height=0.5, controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc: pc.set_default_velocity(0.3) pc.set_default_height(1.0) pc.go_to(1, 0.4, 0.9) pc.go_to(1, 0, 0.9) pc.go_to(1, 0, 1.2) pc.go_to(1, 0.1, 1.2) pc.go_to(1, 0.1, 1.05) pc.go_to(1, 0.1, 1.2) pc.go_to(1, 0.3, 1.2) pc.go_to(1, 0.3, 1.05) pc.go_to(1, 0.3, 1.2) pc.go_to(1, 0.4, 1.2) pc.go_to(1, 0.4, 0.6) pc.go_to(1, -0.4, 0.6) pc.go_to(1, -0.4, 1.5) pc.go_to(1, -0.3, 1.5) pc.go_to(1, -0.15, 1.7) pc.go_to(1, 0, 1.5) pc.go_to(1, 0, 1.3) pc.go_to(1, 0, 1.5) pc.go_to(1, 0.4, 1.5) pc.go_to(1, 0.4, 1.3) pc.go_to(1, 0.4, 1.5) pc.go_to(1, 0.55, 1.7) pc.go_to(1, 0.7, 1.5) pc.go_to(1, 0.8, 1.5) pc.go_to(1, 0.8, 0.4) pc.go_to(1, 0.6, 0.4) pc.go_to(1, 0.6, 0.6) pc.go_to(1, 2.6, 0.6) pc.go_to(1, 2.6, 1.6) pc.go_to(1, 2.1, 1.6) pc.go_to(1, 2.1, 0.6) pc.go_to(1, 3.1, 0.6) pc.go_to(1, 3.1, 1.6) pc.go_to(1, 2.9, 1.4) pc.go_to(1, 3.1, 1.6) pc.go_to(1, 3.1, 0.6) pc.go_to(1, 3.9, 0.6) pc.go_to(1, 3.9, 0.6) pc.go_to(1, 3.4, 1.6) pc.go_to(1, 3.4, 1.2) pc.go_to(1, 3.9, 1.2)
def IPpyservertest(): # Create a TCP/IP socket sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # Bind the socket to the port server_address = ('carbon', 10000) #print >>sys.stderr, 'starting up on %s port %s' % server_address print('starting up on %s port %s' % server_address) sock.bind(server_address) # Listen for incoming connections sock.listen(1) while True: # Wait for a connection print('waiting for a connection', sys.stderr) connection, client_address = sock.accept() try: print('connection from', client_address, sys.stderr) # Receive the data in small chunks and retransmit it while True: data = connection.recv(16) #print('received "%s"' % data, sys.stderr) if data: #print('sending data back to the client', sys.stderr) connection.sendall(data) print(data) #crazyflie example # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie( URI, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) for y in range(10): cf.commander.send_hover_setpoint(0, 0, 0, y / 25) time.sleep(0.1) for _ in range(20): cf.commander.send_hover_setpoint(0, 0, 0, 0.4) time.sleep(0.1) for _ in range(50): cf.commander.send_hover_setpoint( 0.5, 0, 36 * 2, 0.4) time.sleep(0.1) for _ in range(50): cf.commander.send_hover_setpoint( 0.5, 0, -36 * 2, 0.4) time.sleep(0.1) for _ in range(20): cf.commander.send_hover_setpoint(0, 0, 0, 0.4) time.sleep(0.1) for y in range(10): cf.commander.send_hover_setpoint( 0, 0, 0, (10 - y) / 25) time.sleep(0.1) cf.commander.send_stop_setpoint() else: #print('no more data from', client_address, sys.stderr) break finally: # Clean up the connection connection.close()
class LoggingExample: """ Simple logging example class that logs the Stabilizer from a supplied link uri and disconnects after 5s. """ def __init__(self, link_uri, droneNum): """ Initialize and run the example with the specified link_uri """ self._cf = Crazyflie(rw_cache='./cache') # Connect some callbacks from the Crazyflie API self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) print('Connecting to %s' % link_uri) # Try to connect to the Crazyflie self._cf.open_link(link_uri) self.link_uri = link_uri # Variable used to keep main loop occupied until disconnect self.is_connected = True self.droneNum = droneNum 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) try: os.mkdir("testCSV/" + str(self.droneNum)) except: pass try: print("openingFiles") f = open("/testCSV/" + str(droneNum) + '/stab.csv') f.write("stabilizer.roll, stabilizer.pitch, stabilizer.yaw") f.close() print("created stab") f = open("/testCSV/" + str(droneNum) + '/gyro.csv') f.write("gyro.x, gyro.y, gyro.z") f.close() f = open("/testCSV/" + str(droneNum) + '/accel.csv') f.write("acc.x, acc.y, acc.z") f.close() except: pass # The definition of the logconfig can be made before connecting self._lg_stab = LogConfig(name='stab', 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_gyro = LogConfig(name='gyro', period_in_ms=100) 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._lg_accel = LogConfig(name='accel', period_in_ms=100) self._lg_accel.add_variable('acc.x', 'float') self._lg_accel.add_variable('acc.y', 'float') self._lg_accel.add_variable('acc.z', '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) self._cf.log.add_config(self._lg_gyro) self._cf.log.add_config(self._lg_accel) # This callback will receive the data self._lg_stab.data_received_cb.add_callback(self._stab_log_data) self._lg_gyro.data_received_cb.add_callback(self._stab_log_data) self._lg_accel.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) self._lg_gyro.error_cb.add_callback(self._stab_log_error) self._lg_accel.error_cb.add_callback(self._stab_log_error) # Start the logging self._lg_stab.start() self._lg_gyro.start() self._lg_accel.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)) with open('testCSV/' + str(self.droneNum)+ "/" + logconf.name + '.csv','a') as fd: fd.write(convertDictToStr(data) + "\n") 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
len(trajectory_mem.poly4Ds)) return total_duration def run_sequence(cf, trajectory_id, duration): commander = cf.high_level_commander commander.takeoff(1.0, 2.0) time.sleep(3.0) relative = True commander.start_trajectory(trajectory_id, 1.0, relative) time.sleep(duration) commander.land(0.0, 2.0) time.sleep(2) commander.stop() if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: cf = scf.cf trajectory_id = 1 activate_high_level_commander(cf) # activate_mellinger_controller(cf) duration = upload_trajectory(cf, trajectory_id, figure8) print('The sequence is {:.1f} seconds long'.format(duration)) reset_estimator(cf) run_sequence(cf, trajectory_id, duration)
log_conf.data_received_cb.add_callback(outer_callback) print('about to start log') log_conf.start() if __name__ == '__main__': switch_pair_list = { 'formation': ['00', [0, 0, 0]], 'charging': ['00', [0, 0, 0]] } # Initialize the low-level drivers (don't list the debug drivers) cflib.crtp.init_drivers(enable_debug_driver=False) cf_status_lock1 = threading.Lock() # cf_status_lock3 = threading.Lock() # 访问status.current_posture时所需要的锁 # CFStatus, in the final version we may use a list to store it status1 = CFStatus(URI, FlyPosture.flying, cf_status_lock1) status_list = [status1] CFFlyTask.set_switch_pair_list(switch_pair_list) task1 = CFFlyTask(Crazyflie(), status1, [ CFTrajectoryFactory.arch([1, 1, 1], [-1, -1, 1], [-1, 1, 0]), CFTrajectoryFactory.arch([-1, -1, 1], [1, 1, 1], [-1, 1, 0]) ]) DuplicablePositionHlCommander.set_class_status_list(status_list) with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: task1.set_cf_afterword(scf.cf) add_callback_to_singlecf(URI, scf, status1) task1.run() # We take off when the commander is create
class Hover: """Uses vision tracking and PID-control to hover at a given altitude 1. Run camera capture in it's own thread; update altitude 2. Run hover loop in it's own thread; output altitude """ def __init__(self): logger.info("{name} init()".format(name=self.__class__.__name__)) self.settings = shelve.open('/tmp/hover.settings') self.initControl() self.initCamera() self.initTracking() self.initHover() self.initCF() self.settings.close() def initControl(self): """Setup control-flow variables""" self.exit = False def initCamera(self): """Setup camera variables Will prompt the user for feedback. Capable of loading webcam or an ip-camera streaming JPEG. Uses shelve to remember last entered value """ # get settings lastCamUri = self.settings.get('camUri') lastCamRotate = self.settings.get('camRotate') # prompt for camUri, camRotate if lastCamUri: _input = raw_input("Specify camera: 'cam' for webcam, " + "an ip of network camera or <ENTER> for the " + "previous value of '{lastCamUri}':\n".format( lastCamUri=lastCamUri)) if not _input: self.camUri = lastCamUri else: self.camUri = _input else: _input = raw_input("Specify camera: 'cam'/<ENTER> for webcam or " + "an ip of network camera:\n") if not _input: self.camUri = _input else: self.camUri = 'cam' logger.info("CamUri = '{camUri}'".format(camUri=self.camUri)) if lastCamRotate: _input = raw_input("Specify camera rotation or <ENTER> for the " + "previous value of '{lastCamRotate}':\n".format( lastCamRotate=lastCamRotate)) if _input: self.camRotate = int(_input) else: self.camRotate = lastCamRotate else: _input = raw_input("Specify camera rotation or <ENTER> for " + "no rotation':\n") if _input: self.camRotate = int(_input) else: self.camRotate = 0 logger.info("CamRotate = '{camRotate}'"\ .format(camRotate=self.camRotate)) # save settings self.settings['camUri'] = self.camUri self.settings['camRotate'] = self.camRotate # setup camera if self.camUri == "cam": self.cam = Camera() elif '.' not in self.camUri: self.cam = JpegStreamCamera( "http://192.168.1.{ip}:8080/video".format(ip=self.camUri)) else: self.cam = JpegStreamCamera( "http://{ip}:8080/video".format(ip=self.camUri)) # additional values -- for now, just hard coded self.camRes = (800, 600) logger.info("Camera resolution={res}".format(res=self.camRes)) def initTracking(self): """Setup tracking variables Will prompt the user for tracking variables. Uses shelve to remember last entered value """ # get last values lastTrackingColor = self.settings.get('trackingColor') # prompt for override if lastTrackingColor: _input = raw_input("Specify tracking color as (R,G,B)" + "or <ENTER> for previous value " + "{lastTrackingColor}:\n".format( lastTrackingColor=lastTrackingColor)) if _input: self.trackingColor = make_tuple(_input) else: self.trackingColor = lastTrackingColor else: _input = raw_input("Specify tracking color as (R,G,B)" + "or <ENTER> for default of BLUE:\n") if _input: self.trackingColor = make_tuple(_input) else: self.trackingColor = Color.BLUE # save settings self.settings['trackingColor'] = self.trackingColor # additional values self.trackingBlobMin = 5 self.trackingBlobMax = 1000 self.x = -1 self.y = -1 self.target = (300, 400) logger.info(("Tracking color={color}; min={min}; max={max}; " + "target={target}").format(color=self.trackingColor, min=self.trackingBlobMin, max=self.trackingBlobMax, target=self.target)) self.trackingFrameQ = Queue() def initHover(self): self.hoverFrames = [] self.eSum = 0 def initCF(self): self.cfConnected = False self.cf = None self.cfUri = None logger.debug("Initializing Drivers; Debug=FALSE") InitDrivers(enable_debug_driver=False) logger.info("Scanning") available = Scan() logger.info("Found:") for cf in available: logger.debug(cf[0]) if len(available) > 0: self.cfUri = available[0][0] self.cf = Crazyflie() self.cf.connected.add_callback(self.cfOnConnected) self.cf.disconnected.add_callback(self.cfOnDisconnected) self.cf.connection_failed.add_callback(self.cfOnFailed) self.cf.connection_lost.add_callback(self.cfOnLost) logger.info("Crazyflie @{uri} Initialized".format(uri=self.cfUri)) else: logger.info("No Crazyflies found") def cfOnConnected(self, uri): logger.info("{func} {uri}".format(func=inspect.stack()[0][3], uri=uri)) self.cfConnected = True def cfOnDisconnected(self, uri): logger.info("{func} {uri}".format(func=inspect.stack()[0][3], uri=uri)) self.cfConnected = False def cfOnFailed(self, uri): logger.info("{func} {uri}".format(func=inspect.stack()[0][3], uri=uri)) def cfOnLost(self, uri, msg): logger.info("{func} {uri} {msg}".format(func=inspect.stack()[0][3], uri=uri, msg=msg)) def visionLoop(self): while not self.exit: # acquire image img = self.cam.getImage() # exit if we've got nothing if img is None: break # adjust image if self.camRotate != 0: img.rotate(self.camrotate) ''' img = img.resize(self.camRes[0], self.camRes[1]) img = img.rotate90() ''' # blob search colorDiff = img - img.colorDistance(self.trackingColor) blobs = colorDiff.findBlobs(-1, self.trackingBlobMin, self.trackingBlobMax) ''' blobs = img.findBlobs((255,60,60), self.trackingBlobMin, self.trackingBlobMax) ''' # blob find if blobs is not None: self.x = blobs[-1].x self.y = blobs[-1].y # blob show if blobs is not None: # roi = region of interest roiLayer = DrawingLayer((img.width, img.height)) # draw all blobs for blob in blobs: blob.draw(layer=roiLayer) # draw a circle around the main blob roiLayer.circle((self.x, self.y), 50, Color.RED, 2) # apply roi to img img.addDrawingLayer(roiLayer) img = img.applyLayers() img.show() # fps now = datetime.utcnow() self.trackingFrameQ.put(now) if self.trackingFrameQ.qsize() < 30: fps = 0.0 else: fps = 30.0 / (now - self.trackingFrameQ.get()).total_seconds() # logging logger.debug("{func} ({x},{y}) {fps:5.2f}".format( func=inspect.stack()[0][3], x=self.x, y=self.y, fps=fps)) # loop has ened logger.debug("{func} stopped.".format(func=inspect.stack()[0][3])) def hoverLoop(self): while not self.exit: # hover throttled to camera frame rate sleep(1.0 / 30.0) # fps now = datetime.utcnow() self.hoverFrames.append(now) if len(self.hoverFrames) < 30: fps = 0.0 else: fps = 30.0 / (now - self.hoverFrames[-30]).total_seconds() # pid variables kp = (3000.0 / 400.0) ki = 0.18 # determine immediate error e e = self.y - self.target[1] # determine accumulated errors eSum if len(self.hoverFrames) > 1: dt = (now - self.hoverFrames[-2]).total_seconds() else: dt = 0 self.eSum = self.eSum + e * dt # calculate thrust hoverThrust = 37000 thrust = hoverThrust + (kp * e) + (ki * self.eSum) # set the throttle self.setThrust(thrust) # logging logger.debug(("{func} e={e}, dt={dt:0.4f}, eSum={eSum:0.4f}, " + "thrust={thrust}, [{fps:5.2f}]").format( func=inspect.stack()[0][3], e=e, dt=dt, eSum=self.eSum, thrust=thrust, fps=fps)) # loop has ened logger.debug("{func} stopped.".format(func=inspect.stack()[0][3])) def setThrust(self, thrust): """ sets thrust - but if control has exited, will kill thrust """ if self.exit: thrust = 0 if self.cf is not None: self.cf.commander.send_setpoint(0, 0, 0, thrust) def cfOpenLink(self): if self.cf is not None: self.cf.open_link(self.cfUri) logger.info("Linked opened to {uri}".format(uri=self.cfUri)) def start(self): logger.info("Starting VisionLoop") threading.Thread(target=self.visionLoop).start() logger.info("Starting HoverLoop") threading.Thread(target=self.hoverLoop).start() logger.info("Opening Crazyflie link") self.cfOpenLink() raw_input("Press enter to stop") self.stop() def stop(self): # kill our loops self.exit = True # explicitly kill thrust self.setThrust(0) # kill crazyflie if self.cf and self.cfConnected: self.cf.close_link()
curr = [-1 * pose[2][3], -1 * pose[0][3], pose[1][3]] setpoints[closest] = vector_add( grab_setpoint_start, vector_substract(curr, grab_controller_start)) # setpoint = [-1*pose[2][3], -1*pose[0][3], pose[1][3] + 0.3] cf0.commander.send_setpoint(setpoints[0][1], setpoints[0][0], 0, int(setpoints[0][2] * 1000)) cf1.commander.send_setpoint(setpoints[1][1], setpoints[1][0], 0, int(setpoints[1][2] * 1000)) time.sleep(0.02) cf0.commander.send_setpoint(0, 0, 0, 0) cf1.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) if __name__ == '__main__': cflib.crtp.init_drivers(enable_debug_driver=False) with SyncCrazyflie(uri0, cf=Crazyflie(rw_cache='./cache')) as scf0: reset_estimator(scf0) with SyncCrazyflie(uri1, cf=Crazyflie(rw_cache='./cache')) as scf1: reset_estimator(scf1) run_sequence(scf0, scf1) openvr.shutdown()