crubyflie 0.1.0

data/lib/crubyflie/input/joystick_input_reader.rb

@@ -0,0 +1,280 @@
+# -*- 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 'rubygems'
+require 'yaml'
+require 'sdl'
+
+require 'input/input_reader'
+
+module Crubyflie
+
+    # Reads Joystick configuration and specific joystick input
+    # See the default Joystick configuration file in the configs/
+    # folder to have an idea what a configuration file looks like
+    class Joystick < InputReader
+        include Logging
+
+        # Configuration type for Joystick configuration
+        CONFIG_TYPE = "Joystick"
+        # Default SDL joystick input range for axis
+        DEFAULT_INPUT_RANGE = "-32768:32767"
+        # Default Crazyflie min/max angles in degrees
+        DEFAULT_OUTPUT_RANGE = "-30:30"
+        # Default configuration file
+        DEFAULT_CONFIG_PATH = File.join(File.dirname(__FILE__), "..","..","..",
+                                        "configs", "joystick_default.yaml")
+
+        attr_reader :config, :joystick_index
+        # Initializes the Joystick configuration and the SDL library
+        # leaving things ready to read values
+        # @param config_path [String] path to configuration file
+        # @param joystick_index [Integer] the index of the joystick in SDL
+        def initialize(config_path=DEFAULT_CONFIG_PATH, joystick_index = 0)
+            @config = nil
+            @joystick_index = joystick_index
+            @joystick = nil
+            axis, buttons = read_configuration(config_path)
+            super(axis, buttons)
+
+        end
+
+        # Closes the opened resources in SDL
+        def quit
+            SDL.quit()
+        end
+
+        # Parses a YAML Configuration files
+        # @param path [String] Path to the file
+        # @return [Array[Hash]] an array with axis and buttons and
+        #                       and their associated action
+        # @raise [JoystickException] on several configuration error cases
+        def read_configuration(path)
+            begin
+                config_h = YAML.load_file(path)
+            rescue
+                raise JoystickException.new("Could load YAML: #{$!}")
+            end
+
+            if config_h[:type] != CONFIG_TYPE
+                m = "Configuration is not of type #{CONFIG_TYPE}"
+                raise JoystickException.new(m)
+            end
+
+            axis = {}
+            if config_h[:axis].nil?
+                raise JoystickException.new("No axis section")
+            end
+            config_h[:axis].each do |id, axis_cfg|
+                action = axis_cfg[:action]
+                if action.nil?
+                    raise JoystickException.new("Axis #{id} needs an action")
+                end
+
+                axis[id] = action
+            end
+
+            buttons = {}
+            if config_h[:buttons].nil?
+                raise JoystickException.new("No buttons section")
+            end
+            config_h[:buttons].each do |id, button_cfg|
+                action = button_cfg[:action]
+                if action.nil?
+                    raise JoystickException.new("Button #{id} needs an action")
+                end
+                buttons[id] = action
+            end
+
+            @config = config_h
+
+            #logger.info "Loaded configuration correctly (#{path})"
+            return axis, buttons
+        end
+
+        # Init SDL and open the joystick
+        # @raise [JoystickException] if the joystick index is plainly wrong
+        def init_sdl
+            SDL.init(SDL::INIT_JOYSTICK)
+            SDL::Joystick.poll = false
+            n_joy = SDL::Joystick.num
+            logger.info("Joysticks found: #{n_joy}")
+
+            if @joystick_index >= n_joy
+                raise JoystickException.new("No valid joystick index")
+            end
+            @joystick = SDL::Joystick.open(@joystick_index)
+            name      = SDL::Joystick.index_name(@joystick_index)
+            logger.info("Using Joystick: #{name}")
+        end
+        alias_method :init, :init_sdl
+
+        # Used to read the current state of an axis. This is a rather
+        # complicated operation. Raw value is first fit withing the input
+        # range limits, then set to 0 if it falls in the dead zone,
+        # then normalized to the output range that we will like to get (with
+        # special case for thrust, as ranges have different limits), then
+        # we check if the new value falls withing the change rate limit
+        # and modify it if not, finally we re-normalize the thrust if needed
+        # and return the reading, which should be good to be fit straight
+        # into the Crazyflie commander.
+        # @param axis_id [Integer] The SDL joystick axis to be read
+        # @return [Fixnum, Float] the correctly-normalized-value from the axis
+        def read_axis(axis_id)
+            return 0 if !@joystick
+            axis_conf = @config[:axis][axis_id]
+            return 0 if axis_conf.nil?
+            is_thrust = axis_conf[:action] == :thrust
+
+
+            last_poll = axis_conf[:last_poll] || 0
+            last_value = axis_conf[:last_value] || 0
+            invert = axis_conf[:invert] || false
+            calibration = axis_conf[:calibration] || 0
+
+            input_range_s = axis_conf[:input_range] || DEFAULT_INPUT_RANGE
+            ir_start, ir_end = input_range_s.split(':')
+            input_range = Range.new(ir_start.to_i, ir_end.to_i)
+
+            output_range_s = axis_conf[:output_range] || DEFAULT_OUTPUT_RANGE
+            or_start, or_end = output_range_s.split(':')
+            output_range = Range.new(or_start.to_i, or_end.to_i)
+
+            # output value max jump per second. We covert to rate/ms
+            max_chrate = axis_conf[:max_change_rate] || 10000
+            max_chrate = max_chrate.to_f / 1000
+
+            dead_zone = axis_conf[:dead_zone] || "0:0" # % deadzone around 0
+            dz_start, dz_end = dead_zone.split(':')
+            dead_zone_range = Range.new(dz_start.to_i, dz_end.to_i)
+
+            value = @joystick.axis(axis_id)
+            value *= -1 if invert
+            value += calibration
+
+            # Make sure input falls with the expected range
+            if value > input_range.last then value = input_range.last end
+            if value < input_range.first then value = input_range.first end
+            # Dead zone
+
+            if dead_zone_range.first < value && dead_zone_range.last > value
+                value = 0
+            end
+            # Convert
+            if is_thrust
+                value = pre_normalize_thrust(value, input_range, output_range)
+                value = normalize_thrust(value)
+            else
+                value = normalize(value, input_range, output_range)
+            end
+
+            # Check if we change too fast
+            current_time = Time.now.to_f
+            timespan = current_time - last_poll
+            # How many ms have passed since last time
+            timespan_ms = timespan.round(3) * 1000
+            # How much have we changed/ms
+            change = (value - last_value) / timespan_ms.to_f
+
+            # Skip rate limitation if change is positive and this is thurst
+            if !is_thrust || (is_thrust && change <= 0)
+                # If the change rate exceeds  the max change rate per ms...
+                if change.abs > max_chrate
+                    # new value is the max change possible for the timespan
+                    if change > 0
+                        value = last_value + max_chrate * timespan_ms
+                    elsif change < 0
+                        value = last_value - max_chrate * timespan_ms
+                    end
+                end
+            end
+
+            @config[:axis][axis_id][:last_poll] = current_time
+            @config[:axis][axis_id][:last_value] = value
+
+            return value
+        end
+
+        # The thrust axis is disabled for values < 0. What we do here is to
+        # convert it to a thrust 0-100% value first and then make sure it
+        # stays within the limits provided for output range
+        def pre_normalize_thrust(value, input_range, output_range)
+            value = normalize(value, input_range, (-100..100))
+            value = 0 if value < 0
+            if value > output_range.last then value = output_range.last
+            elsif value < output_range.first then value = output_range.first
+            end
+            return value
+        end
+        private :pre_normalize_thrust
+
+
+        # Returns integer from 10.000 to 60.000 which is what the crazyflie
+        # expects
+        def normalize_thrust(value)
+            return normalize(value, (0..100), (9500..60000)).round
+        end
+        private :normalize_thrust
+
+        # Reads the specified joystick button. Since we may want to calibrate
+        # buttons etc, we return an integer. In only reads the button if
+        # the reading has not be done in the last 1 second to avoid
+        # flapping
+        # @param button_id [Integer] the SDL button number
+        # @return [Fixnum] -1 if  the button is not pressed, 1 otherwise
+        def read_button(button_id)
+            return -1 if !@joystick
+
+            last_poll = @config[:buttons][button_id][:last_poll] || 0
+            last_value = @config[:buttons][button_id][:last_value] || -1
+            value = @joystick.button(button_id) ? 1 : -1
+            current_time = Time.now.to_f
+
+            if (current_time - last_poll) > 0.5
+                @config[:buttons][button_id][:last_value] = value
+                @config[:buttons][button_id][:last_poll] = current_time
+                return value
+            else
+                return -1
+            end
+        end
+        private :read_button
+
+        # Called before reading
+        def poll
+            SDL::Joystick.update_all
+        end
+        private :poll
+
+        # Linear-transforms a value in one range to a different range
+        # @param value [Fixnum, Float] the value in the original range
+        # @param from_range [Range] the range from which we want to normalize
+        # @param to_range [Range] the destination range
+        # @return [Float] the linear-corresponding value in the destination
+        #                 range
+        def normalize(value, from_range, to_range)
+            from_w = from_range.size.to_f - 1
+            to_w = to_range.size.to_f - 1
+            from_min = from_range.first.to_f
+            to_min = to_range.first.to_f
+            # puts "#{to_min}+(#{value.to_f}-#{from_min})*(#{to_w}/#{from_w})
+            r = to_min + (value.to_f - from_min) * (to_w / from_w)
+            return r.round(3)
+        end
+    end
+end

data/lib/crubyflie/version.rb

@@ -0,0 +1,22 @@
+# -*- 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
+    # Current gem version
+    VERSION = "0.1.0"
+end

data/lib/crubyflie.rb

@@ -0,0 +1,31 @@
+# -*- 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/>
+
+$: << File.join(File.dirname(__FILE__), 'crubyflie')
+
+require 'crubyflie_logger'
+require 'crazyflie'
+require 'input/joystick_input_reader'
+require 'version'
+
+# The Crubyflie modules wraps all the Crubyflie code so we don't
+# pollute the namespace.
+module Crubyflie
+    $debug = false
+    include Logging
+end

data/spec/commander_spec.rb

@@ -0,0 +1,67 @@
+# -*- 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/commander'
+
+describe Commander do
+
+    before :each do
+        @crazyflie = double("Crazyflie")
+        @queue = Queue.new
+        allow(@crazyflie).to receive(:crtp_queues).and_return({:commander =>
+                                                                  @queue})
+
+        @commander = Commander.new(@crazyflie)
+    end
+
+    describe "#initialize" do
+        it "should initialize the facility" do
+            c = Commander.new(@crazyflie)
+        end
+    end
+
+    describe "#send_sendpoint" do
+        it "should send a sendpoint in normal mode" do
+            expect(@crazyflie).to receive(:send_packet) do |packet, want_answer|
+                want_answer.should == false
+                packet.port.should == Crazyflie::CRTP_PORTS[:commander]
+                packet.channel.should == 0
+                packet.data.size.should == 14
+                packet.data_repack.unpack('eeeS<').should == [1,-2,3,6]
+            end
+
+            @commander.send_setpoint(1,2,3,6)
+        end
+
+        it "should send a sendpoint in xmode" do
+            expect(@crazyflie).to receive(:send_packet) do |packet, want_answer|
+                want_answer.should == false
+                packet.port.should == Crazyflie::CRTP_PORTS[:commander]
+                packet.channel.should == 0
+                packet.data.size.should == 14
+                data = packet.data_repack.unpack('eeeS<')
+                data[0].round(3).should == -0.707
+                data[1].round(4).should == -0.9142
+                data[2].should == 3.0
+                data[3].should == 6
+            end
+
+            @commander.send_setpoint(1,2,3,6, true)
+        end
+    end
+end

data/spec/console_spec.rb

@@ -0,0 +1,76 @@
+# -*- 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/console'
+
+describe Console do
+    before :each do
+        @crazyflie = double("Crazyflie")
+        @queue = Queue.new
+        allow(@crazyflie).to receive(:crtp_queues).and_return({:console =>
+                                                                  @queue})
+
+        @console = Console.new(@crazyflie)
+    end
+    describe "#initialize" do
+        it "should intialize the facility" do
+            c = Console.new(@crazyflie)
+            expect_any_instance_of(Thread).not_to receive(:new)
+        end
+    end
+
+    describe "#read" do
+        it "should read all packets available from the queue" do
+            count = 1
+            p1 = CRTPPacket.new()
+            p1.data = "baa1".unpack('C*')
+            p2 = CRTPPacket.new()
+            p2.data = "baa2".unpack('C*')
+
+            @queue << p1
+            @queue << p2
+
+            @console.read do |message|
+                message.should == "baa#{count}"
+                count += 1
+            end
+            @queue.size.should == 0
+        end
+    end
+
+    describe "#start_reading" do
+        it "should read continuously" do
+            count = 1
+            p1 = CRTPPacket.new()
+            p1.data = "baa1".unpack('C*')
+            p2 = CRTPPacket.new()
+            p2.data = "baa2".unpack('C*')
+
+            @queue << p1
+            @queue << p2
+
+            @console.start_reading do |message|
+                message.should == "baa#{count}"
+                count += 1
+            end
+            sleep 0.3
+            @queue.size.should == 0
+            @console.stop_reading()
+        end
+    end
+end

data/spec/crazyflie_spec.rb

@@ -0,0 +1,176 @@
+# -*- 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'
+
+
+describe Crazyflie do
+    before :all do
+    end
+    before :each do
+        @facility = double("Facility")
+        allow(Commander).to receive(:new).and_return(@facility)
+        allow(Param).to receive(:new).and_return(@facility)
+        allow(Log).to receive(:new).and_return(@facility)
+        allow(Commander).to receive(:new).and_return(@facility)
+        allow(@facility).to receive(:send_setpoint)
+        allow(@facility).to receive(:refresh_toc)
+        allow(@facility).to receive(:start_packet_reader_thread)
+        allow(@facility).to receive(:stop_packet_reader_thread)
+        @link = double("RadioDriver")
+        @uri = 'radio://0/0/1M'
+        allow(RadioDriver).to receive(:new).and_return(@link)
+        allow(@link).to receive(:connect)
+        allow(@link).to receive(:disconnect)
+        allow(@link).to receive(:receive_packet)
+        allow(@link).to receive(:uri).and_return(URI(@uri))
+
+        #allow_any_instance_of(CrubyflieLogger).to receive(:info)
+
+        @cf = Crazyflie.new()
+        @logger = @cf.logger
+
+        @default_pk = CRTPPacket.new(0b00001100, [1,2,3])
+    end
+
+
+    describe "#initialize" do
+        it "should initalize correctly" do
+            Crazyflie::CALLBACKS.each do |cb|
+                @cf.callbacks[cb].size.should >= 0
+            end
+
+            Crazyflie::CRTP_PORTS.keys().each do |port|
+                @cf.crtp_queues[port].should be_an_instance_of Queue
+            end
+        end
+    end
+
+
+    describe "#open_link" do
+        it "should connect to a crazyradio url" do
+            expect(@link).to receive(:receive_packet).and_return(@default_pk)
+            m = "Connection initiated to radio://0/0/1M"
+            m2 = "Disconnected from radio://0/0/1M"
+            m3 = "TOCs extracted from #{@uri}"
+            expect(@logger).to receive(:info).with(m)
+            expect(@logger).to receive(:info).with("Connected!")
+            expect(@logger).to receive(:info).with("Connection ready!")
+            expect(@logger).to receive(:info).with(m2)
+            expect(@logger).to receive(:debug).with(m3)
+
+
+            expect(@facility).to receive(:refresh_toc).twice
+            # only log facility gets this
+            expect(@facility).to receive(:start_packet_reader_thread).once
+            expect(@facility).to receive(:stop_packet_reader_thread).twice
+            @cf.open_link(@uri)
+            @cf.close_link()
+        end
+
+        it "should close the link if something happens" do
+            expect(@link).to receive(:connect).and_raise(Exception)
+            mesg = "Connection failed: Exception"
+            expect(@logger).to receive(:info).at_least(:once)
+            expect(@logger).to receive(:error).with(mesg)
+            expect(@facility).not_to receive(:refresh_toc)
+            @cf.open_link(@uri)
+            @cf.close_link()
+        end
+    end
+
+    describe "#close_link" do
+        it "should close the link" do
+            expect(@facility).to receive(:send_setpoint)
+            expect(@link).to receive(:disconnect)
+            m1 = "Connection initiated to radio://0/0/1M"
+            m2 = "Connection ready!"
+            m3 = "Disconnected from radio://0/0/1M"
+            expect(@logger).to receive(:debug)
+            expect(@logger).to receive(:info).with(m1)
+            expect(@logger).to receive(:info).with(m2)
+            expect(@logger).to receive(:info).with(m3)
+
+            @cf.open_link(@uri)
+            @cf.close_link()
+            @cf.crtp_queues.each do |k,q|
+                q.should be_empty
+            end
+        end
+
+        it "should not break closing a non existing link" do
+            expect(@facility).not_to receive(:send_setpoint)
+            expect_any_instance_of(NilClass).not_to receive(:disconnect)
+            expect(@cf).not_to receive(:puts)
+            expect_any_instance_of(Thread).not_to receive(:kill)
+            expect(@logger).to receive(:info).with("Disconnected from nowhere")
+            @cf.close_link()
+        end
+    end
+
+    describe "#send_packet" do
+        it "should send a packet without expecting answer" do
+            expect(@cf).not_to receive(:setup_retry)
+            expect(@link).to receive(:send_packet).with(@default_pk)
+            expect(@logger).to receive(:info).at_least(:once)
+            expect(@logger).to receive(:debug)
+            @cf.open_link(@uri)
+            @cf.send_packet(@default_pk)
+            @cf.close_link()
+        end
+
+        it "should send a packet and set up a timer when expecting answer" do
+            pk = @default_pk
+            expect(@link).to receive(:send_packet).with(pk).at_least(:twice)
+            expect(@logger).to receive(:info).at_least(:once)
+            expect(@logger).to receive(:debug).at_least(:once)
+            @cf.open_link(@uri)
+            @cf.send_packet(@default_pk, true)
+            sleep 0.5
+            @cf.close_link()
+        end
+    end
+
+    describe "#receive_packet" do
+        it " should receive a packet, trigger callbacks" do
+            proc = Proc.new do
+                puts "port 0 ch 0 callback"
+            end
+
+            proc2 = Proc.new do |pk|
+                puts "Received packet"
+            end
+
+            @cf.callbacks[:received_packet][:log] = proc
+            @cf.callbacks[:received_packet][:log2] = proc2
+
+            allow_any_instance_of(Thread).to receive(:new).and_return(nil)
+            expect(@link).to receive(:receive_packet).and_return(@default_pk)
+            expect(proc).to receive(:call).with(@default_pk).at_least(:once)
+            expect(proc2).to receive(:call).with(@default_pk).at_least(:once)
+            expect(@cf.crtp_queues[:console]).to receive(:<<).once
+            expect(@logger).to receive(:info).at_least(:once)
+            expect(@logger).to receive(:debug)
+            @cf.open_link(@uri)
+            @cf.send(:receive_packet)
+            # Received packet comes on port 0 - console
+
+            @cf.close_link()
+        end
+    end
+end