class Crubyflie::RadioDriver

This layer takes care of connecting to the crazyradio and managing the incoming and outgoing queues. This is done by spawing a thread. It also provides the interface to scan for available crazyflies to which connect

Constants

CALLBACKS

Currently used callbacks that can be passed to connect()

OUT_QUEUE_MAX_SIZE

Default size for the outgoing queue

RETRIES_BEFORE_DISCONNECT

Default number of retries before disconnecting

Attributes

out_queue_max_size[R]
retries_before_disconnect[R]
uri[R]

Public Class Methods

new() click to toggle source

Initialize the driver. Creates new empty queues.

# File lib/crubyflie/driver/radio_driver.rb, line 75
def initialize()
    @uri = nil
    @in_queue = Queue.new()
    @out_queue = Queue.new()
    Thread.abort_on_exception = true
    @radio_thread = nil
    @callbacks = {}
    @crazyradio = nil
    @out_queue_max_size = nil
    @retries_before_disconnect = nil
    @shutdown_thread = false
end

Public Instance Methods

connect(uri_s, callbacks={}, opts={}) click to toggle source

Connect to a Crazyflie in the specified URI @param uri_s [String] a radio uri like radio://<dongle>/<ch>/<rate> @param callbacks [Hash] blocks to call (see CALLBACKS contant values) @param opts [Hash] options. Currently supported

:retries_before_disconnect (defaults to 20) and
:out_queue_max_size (defaults to 50)

@raise [CallbackMissing] when a necessary callback is not provided

(see CALLBACKS constant values)

@raise [InvalidURIException] when the URI is not a valid radio URI @raise [OpenLink] when a link is already open

# File lib/crubyflie/driver/radio_driver.rb, line 98
def connect(uri_s, callbacks={}, opts={})
    # Check if apparently there is an open link

    if @crazyradio
        m = "Active link to #{@uri.to_s}. Disconnect first"
        raise OpenLink.new(m)
    end

    # Parse URI to initialize Crazyradio
    # @todo: better control input. It defaults to 0
    @uri = CrubyflieURI.new(uri_s)
    dongle_number = @uri.dongle.to_i
    channel = @uri.channel.to_i
    rate = @uri.rate
    address = @uri.address

    if address
        begin
            # The official driver does this. Takes address as decimal
            # number, calculate the binary and pack it as 5 byte.
            hex_addr = address.to_i.to_s(16)
            bin_addr = hex_addr.scan(/../).map { |x| x.hex }.pack('C*')
            address = bin_addr.unpack('CCCCC')
        rescue
            raise InvalidURIException.new("Address not valid: #{$!.message}")
        end
    end

    # @todo this should be taken care of in crazyradio
    case rate
    when "250K"
        rate = CrazyradioConstants::DR_250KPS
    when "1M"
        rate = CrazyradioConstants::DR_1MPS
    when "2M"
        rate = CrazyradioConstants::DR_2MPS
    else
        raise InvalidURIException.new("Bad radio rate")
    end

    # Fill in the callbacks Hash
    CALLBACKS.each do |cb|
        if passed_cb = callbacks[cb]
            @callbacks[cb] = passed_cb
        else
            raise CallbackMissing.new("Callback #{cb} mandatory")
        end
    end

    @retries_before_disconnect = opts[:retries_before_disconnect] ||
        RETRIES_BEFORE_DISCONNECT
    @out_queue_max_size = opts[:out_queue_max_size] ||
        OUT_QUEUE_MAX_SIZE

    @retry_forever = @retries_before_disconnect < 0
    m = "Radio driver will retry forever and never disconnect"
    logger.warn(m) if @retry_forever

    # Initialize Crazyradio and run thread
    cradio_opts = {
        :channel => channel,
        :data_rate => rate,
        :address => address
    }
    @crazyradio = Crazyradio.factory(cradio_opts)
    start_radio_thread()

end
disconnect(force=nil) click to toggle source

Disconnects from the crazyradio @param force [TrueClass, FalseClass]. Kill the thread right away, or

wait for out_queue to empty
# File lib/crubyflie/driver/radio_driver.rb, line 170
def disconnect(force=nil)
    kill_radio_thread(force)
    @in_queue.clear()
    @out_queue.clear()

    return if !@crazyradio
    @crazyradio.close()
    @crazyradio = nil
end
get_status() click to toggle source

Get status from the crazyradio. @see Crazyradio#status

# File lib/crubyflie/driver/radio_driver.rb, line 268
def get_status
    return Crazyradio.status()
end
receive_packet(non_block=true) click to toggle source

Fetch a packet from the incoming queue @return [CRTPPacket,nil] a packet from the queue,

or nil when there is none
# File lib/crubyflie/driver/radio_driver.rb, line 208
def receive_packet(non_block=true)
    begin
        return @in_queue.pop(non_block)
    rescue ThreadError
        return nil
    end
end
scan_interface() click to toggle source

List available Crazyflies @return [Array] List of radio URIs where a crazyflie was found @raise [OpenLink] if the Crazyradio is connected already

# File lib/crubyflie/driver/radio_driver.rb, line 228
def scan_interface
    raise OpenLink.new("Cannot scan when link is open") if @crazyradio
    begin
        @crazyradio = Crazyradio.factory()
        results = {}
        @crazyradio[:arc] = 1
        @crazyradio[:data_rate] = Crazyradio::DR_250KPS
        results["250K"] = scan_radio_channels()
        @crazyradio[:data_rate] = Crazyradio::DR_1MPS
        results["1M"]   = scan_radio_channels()
        @crazyradio[:data_rate] = Crazyradio::DR_2MPS
        results["2M"]   = scan_radio_channels()

        uris = []
        results.each do |rate, channels|
            channels.each do |ch|
                uris << "radio://0/#{ch}/#{rate}"
            end
        end
        return uris
    rescue USBDongleException
        raise
    rescue Exception
        retries ||= 0
        logger.error("Error scanning interface: #{$!}")
        retries += 1
        if retries < 2
            logger.error("Retrying")
            sleep 0.5
            retry
        end
        return []
    ensure
        @crazyradio.close() if @crazyradio
        @crazyradio = nil
    end
end
send_packet(packet) click to toggle source

Place a packet in the outgoing queue When not connected it will do nothing @param packet [CRTPPacket] The packet to send

# File lib/crubyflie/driver/radio_driver.rb, line 183
def send_packet(packet)
    return if !@crazyradio
    if (s = @out_queue.size) >= @out_queue_max_size
        if @retry_forever
            m = "Outgoing queue full (#{s} elements) but retry_forever "
            m << "enabled: clearing queue"
            logger.warn(m)
            @out_queue.clear()
        else
            m = "Reached maximum #{s} elements in outgoing queue"
            @callbacks[:link_error_cb].call(m)
            # Force shutdown, otherwise we will hit this error again
            # and go into stack overflow
            # (tecnnically, the link_error_cb should have made sure
            # we are disconnected, but it does not hurt to re-do it)
            disconnect(true)
        end
    end

    @out_queue << packet if !@shutdown_thread
end

Private Instance Methods

kill_radio_thread(force=false) click to toggle source
# File lib/crubyflie/driver/radio_driver.rb, line 362
def kill_radio_thread(force=false)
    if @radio_thread
        if force
            logger.debug("Killing radio thread")
            @radio_thread.kill()
        else
            @shutdown_thread = true
            logger.debug("Shutting down radio thread")
            @radio_thread.join()
        end
        @radio_thread = nil
        @shutdown_thread = false
    end
end
scan_radio_channels(start = 0, stop = 125) click to toggle source

List available Crazyflies in the provided channels @param start [Integer] channel to start @param stop [Intenger] channel to stop @return [Array] list of channels where a Crazyflie was found

# File lib/crubyflie/driver/radio_driver.rb, line 220
def scan_radio_channels(start = 0, stop = 125)
    return @crazyradio.scan_channels(start, stop)
end
start_radio_thread() click to toggle source

Privates The body of the communication thread Sends packets and tries to read the ACK @todo it is long and ugly @todo why the heck do we care here if we need to wait? Should the crazyradio do the waiting?

# File lib/crubyflie/driver/radio_driver.rb, line 279
def start_radio_thread
    @radio_thread = Thread.new do
        Thread.current.priority = 5
        out_data = [0xFF]
        retries = @retries_before_disconnect
        should_sleep = 0
        error = "Unknown"
        while true do
            begin
                ack = @crazyradio.send_packet(out_data)
                # possible outcomes
                # -exception - no usb dongle?
                # -nil - bad comm
                # -AckStatus class
            rescue Exception
                error = "Error talking to Crazyradio: #{$!.to_s}"
                break
            end

            if ack.nil?
                error = "Dongle communication error (ack is nil)"
                break
            end

            # Set this in function of the retries
            quality = (10 - ack.retry_count) * 10
            @callbacks[:link_quality_cb].call(quality)

            # Retry if we have not reached the limit
            if !ack.ack
                # If we have retries left, or we always want to retry
                if retries > 0 || @retry_forever
                    retries -= 1 if !@retry_forever
                    m = "No ack from copter: retries left #{retries}"
                    logger.debug(m)
                    next
                end
                # else just kill the thread
                error = "Too many packets lost"
                break
            else
                retries = @retries_before_disconnect
            end

            # If there is data we queue it in incoming
            # Otherwise we increase should_sleep
            # If there is no data for more than 10 times
            # we will sleep 0.01s when our outgoing queue
            # is empty. Otherwise, we just send what we have
            # of the 0xFF packet
            data = ack.data
            if data.length > 0
                @in_queue << CRTPPacket.unpack(data)
                should_sleep = 0
            else
                should_sleep += 1
            end

            break if @shutdown_thread && @out_queue.empty?()

            begin
                out_packet = @out_queue.pop(true) # non-block
                should_sleep += 1
            rescue ThreadError
                out_packet = CRTPPacket.new(0xFF)
                sleep 0.01 if should_sleep >= 10
            end

            out_data = out_packet.pack
        end
        if !@shutdown_thread
            # If we reach here it means we are dying because of
            # an error. The callback will likely call disconnect, which
            # tries to kills us, but cannot because we are running the
            # callback. Therefore we set @radio_thread to nil and then
            # run the callback.
            @radio_thread = nil
            @callbacks[:link_error_cb].call(error)
        end
    end
end