hybridgroup-crubyflie 0.1.4

data/lib/crubyflie/crazyflie/log.rb

@@ -0,0 +1,383 @@
+# -*- 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 'crazyflie/log_conf.rb'
+
+module Crubyflie
+
+    # An element in the Logging Table of Contents
+    # A LogTOCElement knows what the type of data that comes in a #LogBlock
+    # and is able to initialize the #TOCElement from a TOC Logging packet
+    class LogTOCElement < TOCElement
+
+        # A map between crazyflie C types and ruby directives to
+        # interpret them. This will help parsing the logging data
+        C_RUBY_TYPE_MAP = {
+            1 => {
+                :ctype     => "uint8_t",
+                :directive => 'C',
+                :size      => 1
+            },
+            2 => {
+                :ctype     => "uint16_t",
+                :directive => 'S<',
+                :size      => 2
+            },
+            3 => {
+                :ctype     => "uint32_t",
+                :directive => 'L<',
+                :size      => 4
+            },
+            4 => {
+                :ctype     => "int8_t",
+                :directive => 'c',
+                :size      => '1'
+            },
+            5 => {
+                :ctype     => "int16_t",
+                :directive => 's<',
+                :size      => 2
+            },
+            6 => {
+                :ctype     => "int32_t",
+                :directive => 'l<',
+                :size      => 4
+            },
+            7 => {
+                :ctype     => "float",
+                :directive => 'e',
+                :size      => 4
+            },
+            # Unsupported
+            # 8 => {
+            #     :ctype     => "FP16",
+            #     :directive => '',
+            #     :size      => 2
+            # }
+        }
+
+        # Initializes a Log TOC element, which means interpreting the
+        # data in the packet and calling the parent class
+        # @param data [String] a binary payload
+        def initialize(data)
+            # unpack two null padded strings
+            group, name = data[2..-1].unpack('Z*Z*')
+            ident = data[0].ord()
+            ctype_id = data[1].ord() & 0b1111 # go from 0 to 15
+            ctype = C_RUBY_TYPE_MAP[ctype_id][:ctype]
+            directive = C_RUBY_TYPE_MAP[ctype_id][:directive]
+            access = data[1].ord & 0b00010000 # 0x10, the 5th bit
+
+            super({
+                      :ident => ident,
+                      :group => group,
+                      :name  => name,
+                      :ctype => ctype,
+                      :type_id => ctype_id,
+                      :directive => directive,
+                      :access => access
+                  })
+        end
+
+
+    end
+
+    # A LogBlock represents a piece of logging information that is
+    # received periodically from the Crazyflie after having set the
+    # START_LOGGING command. Each LogBlock will trigger a callback when
+    # a piece of data is received for it.
+    #
+    # Note log blocks are added/removed by the Logging class through the
+    # interface provided. So you should not need to use them directly
+    class LogBlock
+
+        @@block_id_counter = 0
+        attr_reader :ident, :period
+        attr_writer :data_callback
+
+        # Initialize a LogBlock
+        # @param variables [Array] a set of LogConfVariables
+        # @param opts [Hash] current options:
+        #                    :period, in centiseconds (100 = 1s)
+        def initialize(variables, opts={})
+            @variables = variables || []
+            @ident = @@block_id_counter
+            @@block_id_counter += 1
+
+            @period = opts.delete(:period) || 10
+
+            @data_callback = nil
+        end
+
+        # Finds out the binary data by unpacking each of the variables
+        # depending on the number of bites for the declared size
+        # @param data [String] Binary data string
+        def unpack_log_data(data)
+            unpacked_data = {}
+            position = 0
+            @variables.each do |var|
+                fetch_as = var.fetch_as
+                map = LogTOCElement::C_RUBY_TYPE_MAP
+                size = map[fetch_as][:size]
+                directive = map[fetch_as][:directive]
+                name = var.name
+                data_to_unpack = data[position..position + size - 1]
+                value = data_to_unpack.unpack(directive).first
+                unpacked_data[name] = value
+                position += size
+            end
+            @data_callback.call(unpacked_data) if @data_callback
+        end
+    end
+
+    # The logging facility class
+    #
+    # This class is used to read packages received in the Logging port.
+    # It maintains a list of log blocks, which are conveniently added,
+    # or removed and for which logging is started or stopped.
+    # When a packet with new information for log block comes in,
+    # the block in question unpacks the data and triggers a callback.
+    #
+    # In Crubyflie, the Log class includes all the functionality
+    # which is to be found in the Python library LogEntry class (start logging,
+    # add block etc) and the Crazyflie class (callbacks for intialization), so
+    # interfacing with Log should be done through this class primarily.
+    #
+    # Unlike the original Pyhton library, there are no callbacks registered
+    # somewhere else or anything and functions being called from them. In turn,
+    # the Crazyflie class will queue all the logging requests in the @in_queue
+    # while a thread in the Logging class takes care of processing them
+    # and doing the appropiate. This saves us from registering callbacks
+    # in other places and from selecting which data we are to use here.
+    class Log
+        include Logging
+        include CRTPConstants
+
+        attr_reader :log_blocks, :toc
+        # Store the crazyflie, find the incoming packet queue for this
+        # facility and initialize a new TOC
+        # @param crazyflie [Crazyflie]
+        def initialize(crazyflie)
+            @log_blocks = {}
+            @crazyflie = crazyflie
+            @in_queue = crazyflie.crtp_queues[:logging]
+            @toc = TOC.new(@crazyflie.cache_folder, LogTOCElement)
+            @packet_reader_thread = nil
+        end
+
+        # Refreshes the TOC. TOC class implement this step synchronously
+        # so there is no need to provide callbacks or anything
+        def refresh_toc
+            reset_packet = packet_factory()
+            reset_packet.data = [CMD_RESET_LOGGING]
+            port = Crazyflie::CRTP_PORTS[:logging]
+            channel = TOC_CHANNEL
+            @crazyflie.send_packet(reset_packet)
+            @toc.fetch_from_crazyflie(@crazyflie, port, @in_queue)
+        end
+
+        # Creates a log block with the information from a configuration
+        # object.
+        # @param log_conf [LogConf] Configuration for this block
+        # @return [Integer] block ID if things went well,
+        def create_log_block(log_conf)
+            start_packet_reader_thread() if !@packet_reader_thread
+            block = LogBlock.new(log_conf.variables,
+                                 {:period => log_conf.period})
+            block_id = block.ident
+            @log_blocks[block_id] = block
+            packet = packet_factory()
+            packet.data = [CMD_CREATE_BLOCK, block_id]
+            log_conf.variables.each do |var|
+                if var.is_toc_variable?
+                    packet.data << var.stored_fetch_as
+                    packet.data << @toc[var.name].ident
+                else
+                    bin_stored_fetch_as = [var.stored_fetch_as].pack('C')
+                    bin_address = [var.address].pack('L<')
+                    packet.data += bin_stored_fetch_as.unpack('C*')
+                    packet.data += bin_address.unpack('C*')
+                end
+            end
+            logger.debug "Adding block #{block_id}"
+            @crazyflie.send_packet(packet)
+            return block_id
+        end
+
+        # Sends the START_LOGGING command for a given block.
+        # It should be called after #create_toc_log_block. This call
+        # will return immediately, but the provided block will be called
+        # regularly as logging data is received, until #stop_logging is
+        # issued for the same log
+        # Crazyflie. It fails silently if the block does not exist.
+        #
+        # @param block_id [Integer]
+        # @param data_callback [Proc] a block to be called everytime
+        #                             the log data is received.
+        def start_logging(block_id, &data_callback)
+            block = @log_blocks[block_id]
+            block.data_callback = data_callback
+            return if !block
+            start_packet_reader_thread() if !@packet_reader_thread
+            packet = packet_factory()
+            period = block.period
+            packet.data = [CMD_START_LOGGING, block_id, period]
+            logger.debug("Start logging on #{block_id} every #{period*10} ms")
+            @crazyflie.send_packet(packet)
+        end
+
+        # Sends the STOP_LOGGING command to the crazyflie for a given block.
+        # It fails silently if the block does not exist.
+        # @param block_id [Integer]
+        def stop_logging(block_id)
+            block = @log_blocks[block_id]
+            return if !block
+            packet = packet_factory()
+            packet.data = [CMD_STOP_LOGGING, block_id]
+            logger.debug("Stop logging on #{block_id}")
+            @crazyflie.send_packet(packet)
+        end
+
+        # Sends the DELETE_BLOCK command to the Crazyflie for a given block.
+        # It fails silently if the block does not exist.
+        # To be called after #stop_logging.
+        def delete_block(block_id)
+            block = @log_blocks.delete(block_id)
+            return if !block
+            packet = packet_factory()
+            packet.data = [CMD_DELETE_BLOCK, block_id]
+            @crazyflie.send_packet(packet)
+        end
+
+        # A thread that processes the queue of packets intended for this
+        # facility. Recommended to start it after TOC has been refreshed.
+        def start_packet_reader_thread
+            stop_packet_reader_thread()
+            @packet_reader_thread = Thread.new do
+                Thread.current.priority = -4
+                loop do
+                    packet = @in_queue.pop() # block here if nothing is up
+                    # @todo align these two
+                    case packet.channel()
+                    when LOG_SETTINGS_CHANNEL
+                        handle_settings_packet(packet)
+                    when LOG_DATA_CHANNEL
+                        handle_logdata_packet(packet)
+                    when TOC_CHANNEL
+                        # We are refreshing TOC probably
+                        @in_queue << packet
+                        sleep 0.2
+                    else
+                        logger.debug("Log on #{packet.channel}. Cannot handle")
+                        ## in_queue << packet
+                    end
+                end
+            end
+        end
+
+        # Stop the facility's packet processing
+        def stop_packet_reader_thread
+            @packet_reader_thread.kill() if @packet_reader_thread
+            @packet_reader_thread = nil
+        end
+
+        # Finds a log block by id
+        # @param block_id [Integer] the block ID
+        # @return p
+        def [](block_id)
+            @log_blocks[block_id]
+        end
+
+        # Processes an incoming settings packet. Sort of a callback
+        # @param packet [CRTPPacket] the packet on the settings channel
+        def handle_settings_packet(packet)
+            cmd = packet.data[0] # byte 0 of data
+            payload = packet.data_repack()[1..-1] # the rest of data
+
+            block_id = payload[0].ord()
+            #See projects:crazyflie:firmware:comm_protocol#list_of_return_codes
+            # @todo write down error codes in some constant
+            error_st = payload[1].ord() # 0 is no error
+
+            case cmd
+            when CMD_CREATE_BLOCK
+                if !@log_blocks[block_id]
+                    logger.error "No log entry for #{block_id}"
+                    return
+                end
+                if error_st != 0
+                    hex_error = error_st.to_s(16)
+                    mesg = "Error creating block #{block_id}: #{hex_error}"
+                    logger.error(mesg)
+                    return
+                end
+                # Block was created, let's start logging
+                logger.debug "Log block #{block_id} created"
+                # We do not start logging right away do we?
+            when CMD_APPEND_BLOCK
+                logger.debug "Received log settings with APPEND_LOG"
+            when CMD_DELETE_BLOCK
+                logger.debug "Received log settings with DELETE_LOG"
+            when CMD_START_LOGGING
+                if error_st != 0
+                    hex_error = error_st.to_s(16)
+                    mesg = "Error starting to log #{block_id}: #{hex_error}"
+                    logger.error(mesg)
+                else
+                    logger.debug "Logging started for #{block_id}"
+                end
+            when CMD_STOP_LOGGING
+                # @todo
+                logger.debug "Received log settings with STOP_LOGGING"
+            when CMD_RESET_LOGGING
+                # @todo
+                logger.debug "Received log settings with RESET_LOGGING"
+            else
+                mesg = "Received log settings with #{cmd}. Dont now what to do"
+                logger.warn(mesg)
+            end
+
+        end
+        private :handle_settings_packet
+
+        def handle_logdata_packet(packet)
+            block_id = packet.data[0]
+            #logger.debug("Handling log data for block #{block_id}")
+            #timestamp = packet.data[1..3] . pack and re unpack as 4 bytes
+            logdata = packet.data_repack()[4..-1]
+            block = @log_blocks[block_id]
+            if block
+                block.unpack_log_data(logdata)
+            else
+                logger.error "No entry for logdata for block #{block_id}"
+            end
+        end
+        private :handle_logdata_packet
+
+        def packet_factory
+            packet = CRTPPacket.new()
+            packet.modify_header(nil,
+                                 CRTP_PORTS[:logging],
+                                 LOG_SETTINGS_CHANNEL)
+            return packet
+        end
+        private :packet_factory
+
+    end
+end

data/lib/crubyflie/crazyflie/log_conf.rb

@@ -0,0 +1,57 @@
+# -*- 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/>
+
+module Crubyflie
+
+    # Interface for Logging configuration objects
+    class LogConf
+        attr_reader :variables, :data_callback, :period
+        def initialize(variables, data_callback, opts={})
+            @variables = variables
+            @data_callback = data_callback
+            @period = opts[:period] || 20
+        end
+    end
+
+    # Interface for Logging variable configuration objects
+    # this class lists methods to be implemented
+    # Python implementation is in cfclient/utils/logconfigreader.py
+    class LogConfVariable
+
+        attr_reader :name, :stored_as, :fetch_as, :address
+
+        def initialize(name, is_toc, stored_as, fetch_as, address=0)
+            @name = name
+            @is_toc = is_toc
+            @stored_as = stored_as
+            @fetch_as = fetch_as
+            @address = address
+        end
+        # @return [Integer] a byte where the upper 4 bits are the
+        # type indentifier of how the variable is stored and the lower
+        # 4 bits are the type the variable should be fetched as
+        def stored_fetch_as
+            return @stored_as << 4 | (0x0F & @fetch_as)
+        end
+
+        # @return [TrueClass,FalseClass] true if it is stored in the TOC
+        def is_toc_variable?
+            return @is_toc == true
+        end
+    end
+end

data/lib/crubyflie/crazyflie/param.rb

@@ -0,0 +1,220 @@
+# -*- 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'
+
+module Crubyflie
+    # An element in the Parameters Table of Contents
+    class ParamTOCElement < TOCElement
+        # A map between crazyflie C types and ruby directives to
+        # interpret them. This will help parsing the parameter data
+        C_RUBY_TYPE_MAP = {
+            0 => {
+                :ctype     => "int8_t",
+                :directive => 'c',
+                :size      => 1
+            },
+            1 => {
+                :ctype     => "int16_t",
+                :directive => 's<',
+                :size      => 2
+            },
+            2 => {
+                :ctype     => "int32_t",
+                :directive => 'l<',
+                :size      => 4
+            },
+            3 => {
+                :ctype     => "int64_t",
+                :directive => 'q<',
+                :size      => 8
+            },
+            5 => {
+                :ctype     => "FP16",
+                :directive => 'e',
+                :size      => 2
+            },
+            6 => {
+                :ctype     => "float",
+                :directive => 'e',
+                :size      => 4
+            },
+            7 => {
+                :ctype     => "double",
+                :directive => 'E',
+                :size      => 8
+            },
+            8 => {
+                :ctype     => "uint8_t",
+                :directive => 'C',
+                :size      => 1
+            },
+            9 => {
+                :ctype     => "uint16_t",
+                :directive => 'S<',
+                :size      => 2
+            },
+            10 => {
+                :ctype     => "uint32_t",
+                :directive => 'L<',
+                :size      => 4
+            },
+            11 => {
+                :ctype     => "int64_t",
+                :directive => 'Q<',
+                :size      => '8'
+            }
+        }
+
+        # Initializes a Param TOC element, which means interpreting the
+        # data in the packet and calling the parent class
+        # @param data [String] a binary payload
+        # @todo It turns out this is the same as Log. Only the type conversion
+        # changes
+        def initialize(data)
+            # The group and name are zero-terminated strings from the 3rd byte
+            group, name = data[2..-1].unpack('Z*Z*')
+            ident = data[0].ord()
+            ctype_id = data[1].ord() & 0b1111  #from 0 to 15
+            ctype = C_RUBY_TYPE_MAP[ctype_id][:ctype]
+            directive = C_RUBY_TYPE_MAP[ctype_id][:directive]
+            access = data[1].ord() & 0b00010000 # 5th bit
+
+            super({
+                      :ident => ident,
+                      :group => group,
+                      :name  => name,
+                      :ctype => ctype,
+                      :type_id => ctype_id,
+                      :directive => directive,
+                      :access => access
+                  })
+        end
+    end
+
+    # The parameter facility. Used to retrieve the table of contents,
+    # set the value of a parameter and read the value of a parameter
+    class Param
+        include Logging
+        include CRTPConstants
+
+        attr_reader :toc
+        # Initialize the parameter facility
+        # @param crazyflie [Crazyflie]
+        def initialize(crazyflie)
+            @crazyflie = crazyflie
+            @in_queue = crazyflie.crtp_queues[:param]
+            @toc = TOC.new(@crazyflie.cache_folder, ParamTOCElement)
+        end
+
+        # Refreshes the TOC. It only returns when it is finished
+        def refresh_toc
+            channel = TOC_CHANNEL
+            port = Crazyflie::CRTP_PORTS[:param]
+            @toc.fetch_from_crazyflie(@crazyflie, port, @in_queue)
+        end
+
+        # Set the value of a paremeter. This call will only return after
+        # a ack response has been received, or will timeout if
+        # #CRTPConstants::WAIT_PACKET_TIMEOUT is reached.
+        # @param name [String] parameter group.name
+        # @param value [Numeric] a value. It must be packable as binary data,
+        #                                the type being set in the params TOC
+        # @param block [Proc] an optional block that will be called with the
+        #                     response CRTPPacket received. Otherwise will
+        #                     log to debug
+        def set_value(name, value, &block)
+            element = @toc[name]
+            if element.nil?
+                logger.error "Param #{name} not in TOC!"
+                return
+            end
+
+            ident = element.ident
+            packet = CRTPPacket.new()
+            packet.modify_header(nil, CRTP_PORTS[:param],
+                                 PARAM_WRITE_CHANNEL)
+            packet.data = [ident]
+            packet.data += [value].pack(element.directive()).unpack('C*')
+            @in_queue.clear()
+            @crazyflie.send_packet(packet, true) # expect answer
+
+            response = wait_for_response()
+            return if response.nil?
+
+            if block_given?
+                yield response
+            else
+                mesg = "Got answer to setting param '#{name}' with '#{value}'"
+                logger.debug(mesg)
+            end
+        end
+
+        # Request an update for a parameter and call the provided block with
+        # the value of the parameter. This call will block until the response
+        # has been received or the #CRTPConstants::WAIT_PACKET_TIMEOUT is
+        # reached.
+        # @param name [String] a name in the form group.name
+        # @param block [Proc] a block to be called with the value as argument
+        def get_value(name, &block)
+            element = @toc[name]
+            if element.nil?
+                logger.error "Cannot update #{name}, not in TOC"
+                return
+            end
+            packet = CRTPPacket.new()
+            packet.modify_header(nil, CRTP_PORTS[:param],
+                                 PARAM_READ_CHANNEL)
+            packet.data = [element.ident]
+            @in_queue.clear()
+            @crazyflie.send_packet(packet, true)
+
+            # Pop packages until mine comes up
+            begin
+                response = wait_for_response()
+                return if response.nil?
+
+                ident = response.data()[0]
+                if ident != element.ident()
+                    mesg = "Value expected for element with ID #{element.ident}"
+                    mesg << " but got for element with ID #{ident}. Requeing"
+                    logger.debug(mesg)
+                end
+            end until ident == element.ident()
+
+            value = response.data_repack()[1..-1]
+            value = value.unpack(element.directive).first
+            yield(value)
+        end
+        alias_method :request_param_update, :get_value
+
+        def wait_for_response
+            begin
+                wait = WAIT_PACKET_TIMEOUT
+                response = timeout(wait, WaitTimeoutException) do
+                    @in_queue.pop()
+                end
+                return response
+            rescue WaitTimeoutException
+                logger.error("Param: waited too long to get response")
+                return nil
+            end
+        end
+        private :wait_for_response
+    end
+end