Example #1
0
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"]))
Example #2
0
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()
Example #4
0
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()
Example #6
0
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))
Example #7
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()
Example #10
0
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()
Example #12
0
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()
Example #14
0
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"])
        )
Example #15
0
    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
Example #18
0
    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
Example #19
0
    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
Example #20
0
 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
Example #21
0
    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()
Example #26
0
    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()
Example #27
0
 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
Example #30
0
    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
Example #31
0

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
Example #32
0
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
Example #33
0
    #    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))
Example #34
0
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()
Example #35
0
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
Example #37
0
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()
Example #38
0
    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
Example #39
0
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
Example #40
0
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)
Example #41
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
Example #42
0
    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)
Example #43
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
        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)
Example #45
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")
Example #48
0
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}')
Example #50
0
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()
Example #51
0
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)
Example #52
0
# 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
Example #53
0
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)
Example #54
0
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)
Example #55
0
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()
Example #56
0
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()