hybridgroup-crubyflie 0.1.4

data/lib/crubyflie/crazyflie/toc.rb

@@ -0,0 +1,239 @@
+# -*- coding: utf-8 -*-
+# Copyright (C) 2013 Hector Sanjuan
+
+# This file is part of Crubyflie.
+
+# Crubyflie is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# Crubyflie is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with Crubyflie.  If not, see <http://www.gnu.org/licenses/>
+
+require 'timeout'
+require 'crazyflie/toc_cache'
+
+module Crubyflie
+
+    # Base class for a TocElement. To be extended by specific classes
+    class TOCElement
+        # Initializes a TOC element
+        # @param element_h [Hash] indicates :ident, :group, :name, :ctype,
+        #                         :rtype, :access
+        attr_reader :ident, :group, :name, :ctype, :type_id, :directive, :access
+        def initialize(element_h)
+            @ident = element_h.delete(:ident) || 0
+            @group  = element_h.delete(:group) || ""
+            @name  = element_h.delete(:name) || ""
+            @ctype = element_h.delete(:ctype) || ""
+            @type_id = element_h.delete(:type_id) || -1
+            @directive = element_h.delete(:directive) || ""
+            @access = element_h.delete(:access) || 0
+        end
+    end
+
+    # A Table Of Contents
+    # It is a hash that stores a group index.
+    # Each group is a Hash indexed by element ID that stores TOC element
+    class TOC
+        include Logging
+        include CRTPConstants
+        attr_reader :toc
+        # Initializes the hash
+        # @param cache_folder [String] where is the cache for this toc stored
+        # @param element_class [Class] the class type we should instantiate
+        #                              TOC elements to, when fetching them from
+        #                              the crazyflie
+        def initialize(cache_folder=nil, element_class=TOCElement)
+            @toc = {}
+            @cache = TOCCache.new(cache_folder)
+            @element_class = element_class
+        end
+
+        # Get a TOC element
+        # @param name_or_id [String,Symbol] name or ident of the element
+        # @param mode [Symbol] get it :by_name, :by_id, :both
+        # @return [TocElement, nil] the element or nil if not found
+        def [](name_or_id, mode=:both)
+            by_name_element = nil
+            by_id_element   = nil
+            # Find by name
+            if [:both, :by_name].include?(mode)
+                group = name = nil
+                if name_or_id.is_a?(String)
+                    group, name = name_or_id.split(".", 2)
+                end
+
+                if name.nil?
+                    name = group
+                    group = nil
+                end
+
+                if group
+                    gr = @toc[group]
+                    by_name_element = gr[name] if gr
+                else
+                    @toc.each do |group_name, group|
+                        candidate = group[name]
+                        by_name_element = candidate if candidate
+                        break if candidate
+                    end
+                end
+            end
+
+            if [:both, :by_id].include?(mode)
+                @toc.each do |group_name, group|
+                    group.each do |name, element|
+                        by_id_element = element if element.ident == name_or_id
+                        break if by_id_element
+                    end
+                end
+            end
+
+            return by_name_element || by_id_element
+        end
+
+        # Insert a TOC element in the TOC
+        # @param element [TocElement] the name of the element group
+        def insert(element)
+            group = element.group
+            name = element.name
+            @toc[group] = {} if @toc[group].nil?
+            @toc[group][name] = element
+        end
+        alias_method :<<, :insert
+
+        # Saves this TOC into cache
+        def export_to_cache(crc)
+            @cache.insert(crc, @toc)
+        end
+
+        # Retrieves this TOC from the cache
+        def import_from_cache(crc)
+            @toc = @cache.fetch(crc)
+        end
+
+        # Produce a simple string representation of the TOC
+        # @return [String] a pretty string
+        def to_s
+            s = ""
+            @toc.each do |group,v|
+                s << "- #{group}\n"
+                v.each do |name, elem|
+                    s << "    * #{name} (#{elem.ctype}/##{elem.type_id})\n"
+                end
+            end
+            return s
+        end
+
+        # Fetches a TOC from crazyflie in a synchronous way
+        # Instead of a TOCFetcher (as in the python library), we just found
+        # easier to take advantage of the facility queues and read them
+        # (and block when there is nothing to read) to initialize the TOC.
+        # Doing it this way not only saves to have to register and chain
+        # callbacks unrelated places (as Crazyflie class), but also to
+        # reorder incoming TOCElement packages if they come unordered (we
+        # just requeue them). This might happen if a package needs to be resent
+        # and the answer for the next one comes earlier.
+        #
+        # This function should be preferably called before starting any
+        # other activities (send/receive threads) in the relevant facilities.
+        # @param crazyflie [Crazyflie] used to send packages
+        # @param port [Integer] the port to send the packages
+        # @param in_queue [Integer] a queue on which the responses to the
+        #                           sent packages are queued
+        def fetch_from_crazyflie(crazyflie, port, in_queue)
+            # http://wiki.bitcraze.se/projects:crazyflie:firmware:comm_protocol
+            # #table_of_content_access
+            packet = CRTPPacket.new(0, [CMD_TOC_INFO])
+            packet.modify_header(nil, port, TOC_CHANNEL)
+            in_queue.clear()
+
+            crazyflie.send_packet(packet, true)
+            response = in_queue.pop() # we block here if none :)
+            while response.channel != TOC_CHANNEL do
+                in_queue << response
+                logger.debug "Got a non-TOC packet. Requeueing..."
+                Thread.pass
+                # @todo timeout
+                response = in_queue.pop()
+            end
+            data = response.data
+            # command = data[0]
+            # Repack the payload
+            payload = response.data_repack[1..-1] # get rid of byte 0
+            # The crc comes in an unsigned int (L) in little endian (<)
+            total_toc_items, crc = payload[0..5].unpack('CL<')
+            hex_crc = crc.to_s(16)
+
+            logger.debug "TOC crc #{hex_crc}, #{total_toc_items} items"
+            import_from_cache(hex_crc)
+
+            if !@toc.nil? # in cache so we can stop here
+                logger.debug "TOC found in cache"
+                return
+            end
+
+            logger.debug "Not in cache"
+            @toc = {}
+            # We proceed to request all the TOC elements
+            requested_item = 0
+            while requested_item < total_toc_items do
+                wait = WAIT_PACKET_TIMEOUT
+                request_toc_element(crazyflie, requested_item, port)
+
+                begin
+                    response = timeout(wait, WaitTimeoutException) do
+                        response = in_queue.pop() # block here
+                    end
+                rescue
+                    retries ||= 0
+                    retries += 1
+                    retry if retries < 2
+                    logger.error("Timeout reached waiting for TOC element")
+                    raise $!
+                end
+                if response.channel != TOC_CHANNEL
+                    # Requeue
+                    in_queue << response
+                    mesg =  "Got a non-TOC packet on #{response.channel}."
+                    mesg << " Requeueing..."
+                    logger.debug(mesg)
+                    Thread.pass
+                    next
+                end
+                payload = response.data_repack()[1..-1] # leave byte 0 out
+                toc_elem = @element_class.new(payload)
+                if (a = requested_item) != (b = toc_elem.ident())
+                    logger.debug "#{port}: Expected #{a}, but got #{b}"
+                    if b > a
+                        logger.debug "Requeing"
+                        # this way we are ordering items
+                        in_queue << response
+                    end
+                    next
+                end
+
+                insert(toc_elem)
+                logger.debug("Added #{toc_elem.ident} to TOC")
+                requested_item += 1
+            end
+            export_to_cache(hex_crc)
+        end
+
+
+        def request_toc_element(crazyflie, index, port)
+            packet = CRTPPacket.new(0, [CMD_TOC_ELEMENT, index])
+            packet.modify_header(nil, port, TOC_CHANNEL)
+            crazyflie.send_packet(packet, true)
+        end
+        private :request_toc_element
+
+    end
+end

data/lib/crubyflie/crazyflie/toc_cache.rb

@@ -0,0 +1,87 @@
+# -*- coding: utf-8 -*-
+# Copyright (C) 2013 Hector Sanjuan
+
+# This file is part of Crubyflie.
+
+# Crubyflie is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# Crubyflie is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with Crubyflie.  If not, see <http://www.gnu.org/licenses/>
+
+
+# Keeps a serialized version of a TOC in disk so there is no need to
+# query the Crazyflie. The files are named with the TOC CRC.
+
+require 'fileutils'
+
+module Crubyflie
+
+    # Table of contents can be saved to disk and re-read from there
+    # based on the CRC that they have attached. This class
+    # is used for that
+    class TOCCache
+
+        # Initializes the cache directory
+        # @param folder [String] a path to the folder
+        def initialize(folder=nil)
+            @folder = folder
+            return if !@folder
+            if !File.exist?(folder)
+                begin
+                    FileUtils.mkdir_p(folder)
+                rescue Errno::EACCES
+                    warn "Deactivating cache. Cannot create folder"
+                    @folder = nil
+                end
+            elsif !File.directory?(folder)
+                @folder = nil
+                warn "Deactivating cache. Folder is not a directory"
+                return
+            else
+                begin
+                    test_f = File.join(folder, 'test')
+                    FileUtils.touch(test_f)
+                    FileUtils.rm(test_f)
+                rescue Errno::EACCES
+                    @folder = nil
+                    warn "Deactivating cache. Cannot write to folder"
+                end
+            end
+        end
+
+        # Fetches a record from the cache
+        # @param crc [String] the CRC of the TOC
+        # @return [TOC,nil] A TOC if found
+        def fetch(crc)
+            return nil if !@folder
+            begin
+                File.open(File.join(@folder, crc), 'r') do |f|
+                    Marshal.load(f.read)
+                end
+            rescue Errno::ENOENT, Errno::EACCES
+                nil
+            end
+        end
+
+        # Saves a record to the cache
+        # @param crc [String] the CRC of the TOC
+        # @param toc [TOC] A TOC
+        def insert(crc, toc)
+            return if !@folder
+            begin
+                File.open(File.join(@folder, crc), 'w') do |f|
+                    f.write(Marshal.dump(toc))
+                end
+            rescue Errno::ENOENT, Errno::EACCES
+            end
+        end
+    end
+end

data/lib/crubyflie/crazyflie.rb

@@ -0,0 +1,282 @@
+# -*- coding: utf-8 -*-
+# Copyright (C) 2013 Hector Sanjuan
+
+# This file is part of Crubyflie.
+
+# Crubyflie is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# Crubyflie is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with Crubyflie.  If not, see <http://www.gnu.org/licenses/>
+
+require 'thread'
+
+require 'driver/radio_driver'
+require 'crazyflie/toc'
+require 'crazyflie/console'
+require 'crazyflie/commander'
+require 'crazyflie/log'
+require 'crazyflie/param'
+
+module Crubyflie
+
+    # This is the main entry point for interacting with a Crazyflie
+    # It allows to connect to the Crazyflie on the specified radio URL
+    # and make use of the different facilities: log, param, commander and
+    # console. Facilities are instantiated and packages delivered to their
+    # queues from here. For example, you can use
+    # the @crazyflie.commander.set_sendpoint() to send a new setpoint
+    # to a Crazyflie
+    class Crazyflie
+        include Logging
+        include CRTPConstants
+
+        # Groups of callbacks available
+        CALLBACKS = [
+                     :received_packet,
+                     :disconnected,
+                     :connection_failed,
+                     :connection_initiated,
+                     :connection_setup_finished,
+                     :link
+                    ]
+
+        attr_accessor :callbacks
+        attr_reader :cache_folder, :commander, :console, :param, :log
+        attr_reader :crtp_queues, :link
+        # Initialize a Crazyflie by registering default received-packet
+        # callbacks and intializing Queues for every facility.
+        # Packets will be queued for each facility depending on their port
+        # @param cache_folder [String] folder path to store logging TOC cache
+        def initialize(cache_folder=nil)
+            @cache_folder = cache_folder
+            # Callbacks will fire in some specific situations
+            # Specially when receiving packages
+            @callbacks = {}
+            CALLBACKS.each do |cb|
+                @callbacks[cb] = {}
+            end
+            register_default_callbacks()
+
+            @crtp_queues = {}
+            CRTP_PORTS.keys().each do |k|
+                @crtp_queues[k] = Queue.new
+            end
+
+            # Thread that regularly checks for packages
+            @receive_packet_thread = nil
+            @retry_packets_thread = nil
+
+            # A hash with keys "port_<portnumber>"
+            @retry_packets = {}
+
+            @commander = Commander.new(self)
+            @console   = Console.new(self)
+            @param     = Param.new(self)
+            @log       = Log.new(self)
+
+            @link = nil
+        end
+
+
+        # Connect to a Crazyflie using the radio driver
+        # @param uri [String] radio uri to connect to
+        def open_link(uri)
+            call_cb(:connection_initiated, uri)
+            begin
+                @link = RadioDriver.new()
+                link_cbs = {
+                    :link_quality_cb => @callbacks[:link][:quality],
+                    :link_error_cb  => @callbacks[:link][:error]
+                }
+
+                @link.connect(uri, link_cbs)
+                @callbacks[:received_packet][:connected] = Proc.new do |packet|
+                    logger.info "Connected!"
+                    @callbacks[:received_packet].delete(:connected)
+                end
+                receive_packet_thread()
+                sleep 0.5 # Allow setup and failures
+                setup_connection() if @link
+            rescue Exception
+                #logger.warn $!.backtrace.join("\n")
+                call_cb(:connection_failed, $!.message)
+                close_link()
+            end
+        end
+
+        # Close the link and clean up
+        # Attemps to disconnect from the crazyflie.
+        def close_link
+            @commander.send_setpoint(0,0,0,0) if @link
+            sleep 0.05
+            uri = @link ? @link.uri.to_s : "nowhere"
+            @link.disconnect(force=true) if @link
+            @link = nil
+            @receive_packet_thread.kill() if @receive_packet_thread
+            @receive_packet_thread = nil
+            @retry_packets_thread.kill() if @retry_packets_thread
+            @log.stop_packet_reader_thread()
+            @retry_packets.clear()
+            @crtp_queues.each do |k,q|
+                q.clear()
+            end
+            call_cb(:disconnected, uri)
+        end
+
+        # Checks if there is an open link
+        # @return [TrueClass, FalseClass] true if there is an open link
+        def active?
+            return !@link.nil?
+        end
+
+        # Calls #RadioDriver::scan_interface()
+        # @return [Array] List of radio URIs where a crazyflie was found
+        def scan_interface
+            if @link
+                logger.error "Cannot scan when link is open. Disconnect first"
+                return []
+            end
+            begin
+                RadioDriver.new().scan_interface()
+            rescue Exception
+                logger.error("Cannot scan interfaces: #{$!}")
+                return []
+            end
+        end
+
+        # Send a packet
+        # @param packet [CRTPPacket] packet to send
+        # @param expect_answer [TrueClass, FalseClass] if set to true, a timer
+        #                                              will be set up to
+        #                                              resend the package if
+        #                                              no response has been
+        #                                              received in 0.1secs
+        def send_packet(packet, expect_answer=false)
+            return if @link.nil?
+            @link.send_packet(packet)
+            setup_retry(packet) if expect_answer
+        end
+
+
+        def receive_packet
+            packet = @link.receive_packet(false) # block here
+            return if packet.nil?
+            call_cb(:received_packet, packet)
+            port = packet.port
+            facility = CRTP_PORTS.invert[port]
+            queue = @crtp_queues[facility]
+            if queue then queue << packet
+            else logger.warn "No queue for packet on port #{port}" end
+        end
+        private :receive_packet
+
+        def setup_connection
+            # Logging will send other packets such RESET_LOGGING
+            # We make sure to handle them by running the thread
+            # while we refresh the Log TOC
+            #logger.debug("Setup connection: Log TOC refresh")
+            @log.start_packet_reader_thread()
+            @log.refresh_toc()
+            @log.stop_packet_reader_thread()
+            #logger.debug("Setup connection: Param TOC refresh")
+            @param.refresh_toc()
+            call_cb(:connection_setup_finished, @link.uri.to_s)
+        end
+        private :setup_connection
+
+        def receive_packet_thread
+            @receive_packet_thread = Thread.new do
+                Thread.current.priority = -2
+                loop do
+                    if @link.nil? then sleep 1
+                    else receive_packet(); Thread.pass() end
+                end
+            end
+
+            # This threads resends packets for which no answer has been
+            # received.
+            @retry_packets_thread = Thread.new do
+                Thread.current.priority = -5
+
+                loop do
+                    @retry_packets.each do |k,v|
+                        now = Time.now.to_f
+                        ts = v[:timestamp]
+                        if now - ts >= 0.2
+                            pk = v[:packet]
+                            logger.debug("Replay on #{pk.port}:#{pk.channel}")
+                            send_packet(v[:packet], true)
+                        end
+                    end
+                    sleep 0.2
+                end
+            end
+        end
+        private :receive_packet_thread
+
+        def call_cb(cb, *args)
+            @callbacks[cb].each do |name, proc|
+                proc.call(*args)
+            end
+        end
+        private :call_cb
+
+        # Register some callbacks which are default
+        def register_default_callbacks
+            @callbacks[:received_packet][:delete_timer] = Proc.new do |pk|
+                sym = "port_#{pk.port}".to_sym
+                @retry_packets.delete(sym)
+                #logger.debug("Packet: #{pk.port}: #{pk.channel}")
+            end
+
+            @callbacks[:disconnected][:log] = Proc.new do |uri|
+                logger.info "Disconnected from #{uri}"
+            end
+
+            @callbacks[:connection_failed][:log] = Proc.new do |m|
+                logger.error "Connection failed: #{m}"
+            end
+
+            @callbacks[:connection_initiated][:log] = Proc.new  do |uri|
+                logger.info "Connection initiated to #{uri}"
+            end
+
+            @callbacks[:connection_setup_finished][:log] = Proc.new do |uri|
+                logger.debug "TOCs extracted from #{uri}"
+                logger.info "Connection ready!"
+            end
+
+            # The message is an integer from 0 - 100
+            @callbacks[:link][:quality] = Proc.new  do |m|
+                #logger.debug "Link quality #{m}"
+            end
+
+            @callbacks[:link][:error] = Proc.new  do |m|
+                logger.error "Link error: #{m}"
+                close_link()
+            end
+
+        end
+        private :register_default_callbacks
+
+        # A retry will fire for this request
+        def setup_retry(packet)
+            retry_sym = "port_#{packet.port}".to_sym
+            @retry_packets[retry_sym] = {
+                :packet => packet,
+                :timestamp => Time.now.to_f
+            }
+
+        end
+        private :setup_retry
+
+    end
+end