hybridgroup-crubyflie 0.1.4

data/spec/joystick_cfg.yaml

@@ -0,0 +1,44 @@
+:type: "Joystick"
+:axis:
+  0:
+    :description: "Roll Axis"
+    :action: :roll
+    :input_range: "-32768:32767"
+    :output_range: "-30:30"
+    :max_change_rate: 600
+    :dead_zone: "-100:100"
+    :invert: false
+    :calibration: 0
+  1:
+    :description: "Pitch Axis"
+    :action: :pitch
+    :input_range: "-32768:32767"
+    :output_range: "-30:30"
+    :max_change_rate: 600
+    :dead_zone: "0:0"
+    :invert: true
+    :calibration: 0
+  2:
+    :description: "Yaw Axis"
+    :action: :yaw
+    :input_range: "-32768:32767"
+    :output_range: "-30:30"
+    :max_change_rate: 800
+    :dead_zone: "-1000:1000"
+    :invert: false
+    :calibration: 0
+  3:
+    :description: "Thrust axis"
+    :action: :thrust
+    :input_range: "-32768:32767"
+    :output_range: "0:80"
+    :max_change_rate: 40000
+    :dead_zone: "-100:100"
+    :invert: true
+    :calibration: 0
+
+:buttons:
+  0:
+    :action: :switch_xmode
+  1:
+    :action: :close_link

data/spec/joystick_input_reader_spec.rb

@@ -0,0 +1,323 @@
+# -*- 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 'input/joystick_input_reader'
+
+describe Joystick do
+
+    before :each do
+        @sdl_joystick = double("Joystick")
+        allow(SDL::Joystick).to receive(:update_all)
+        allow(SDL).to receive(:init).with(SDL::INIT_JOYSTICK)
+        allow(SDL::Joystick).to receive(:num).and_return(3)
+        allow(SDL::Joystick).to receive(:open).and_return(@sdl_joystick)
+        allow(SDL::Joystick).to receive(:index_name).and_return("My Joystick")
+        allow(SDL::Joystick).to receive(:poll).with(false)
+        expect(SDL::Joystick).not_to receive(:poll).with(true)
+        @path = File.join(File.dirname(__FILE__), 'joystick_cfg.yaml')
+
+        @joystick = Joystick.new(@path)
+
+        @logger = @joystick.logger
+        allow(@logger).to receive(:info)
+        @joystick.init()
+    end
+
+    describe "#initialize" do
+        it "should initialize a valid joystick supposing things go well" do
+            @joystick.axis.should == {
+                0 => :roll,
+                1 => :pitch,
+                2 => :yaw,
+                3 => :thrust
+            }
+
+            @joystick.buttons.should == {
+                0 => :switch_xmode,
+                1 => :close_link
+            }
+        end
+    end
+
+    describe "#read_configuration" do
+        it "should read a configuration correctly" do
+            cfg = {
+                :type => "Joystick",
+                :axis => {0 => {:action => :yaw}},
+                :buttons => {0 => {:action => :roll}}
+            }
+            expect(YAML).to receive(:load_file).and_return(cfg)
+
+            axis, buttons = @joystick.read_configuration('path')
+            @joystick.config[:type].should == "Joystick"
+            axis.should == { 0 => :yaw }
+            buttons.should == { 0 => :roll }
+        end
+
+        it "should raise exception if it cannot load the configuration" do
+            expect {
+                @joystick.read_configuration('baa')
+            }.to raise_exception(JoystickException)
+        end
+
+        it "should raise exception if the configuration type is bad" do
+            cfg = {
+                :type => "BBOBOBO",
+                :axis => {0 => {:action => :yaw}},
+                :buttons => {0 => {:action => :roll}}
+            }
+            expect(YAML).to receive(:load_file).and_return(cfg)
+            m = "Configuration is not of type Joystick"
+            expect {
+                @joystick.read_configuration('baa')
+            }.to raise_exception(JoystickException, m)
+        end
+
+        it "should raise exception if the axis or buttons are missing" do
+            cfg = {
+                :type => "Joystick",
+                :buttons => {0 => {:action => :yaw}},
+            }
+            expect(YAML).to receive(:load_file).and_return(cfg)
+            expect {
+                @joystick.read_configuration('baa')
+            }.to raise_exception(JoystickException, "No axis section")
+        end
+
+        it "should raise exception if an axis has no action" do
+            cfg = {
+                :type => "Joystick",
+                :axis => {0 => {:invert => true}},
+                :buttons => {0 => {:action => :roll}}
+            }
+            expect(YAML).to receive(:load_file).and_return(cfg)
+            expect {
+                @joystick.read_configuration('baa')
+            }.to raise_exception(JoystickException, "Axis 0 needs an action")
+        end
+    end
+
+    describe "#init_sdl" do
+        it "should raise an exception if the joystick index is invalid" do
+            expect {
+                js = Joystick.new(@path, 33)
+                allow(js.logger).to receive(:info)
+                js.init()
+            }.to raise_exception(JoystickException, "No valid joystick index")
+        end
+
+        it "should open the joystick" do
+            expect(SDL::Joystick).to receive(:open).with(2)
+            js = Joystick.new(@path, 2)
+            allow(js.logger).to receive(:info)
+            js.init()
+        end
+    end
+
+    describe "#read_axis" do
+        it "should read the axis value with all the defaults" do
+            config = {
+                :type => "Joystick",
+                :axis => {
+                    8 =>{
+                        :action => :test,
+                    }
+                }
+            }
+            expect(YAML).to receive(:load_file).and_return(config)
+            expect(@sdl_joystick).to receive(:axis).with(8).and_return(-32768)
+            @joystick.read_configuration('baa')
+            value = @joystick.read_axis(8)
+            value.should == -30
+        end
+
+        it "should put values out of range in range" do
+            config = {
+                :type => "Joystick",
+                :axis => {
+                    8 =>{
+                        :action => :test,
+                    }
+                }
+            }
+            expect(YAML).to receive(:load_file).and_return(config)
+            expect(@sdl_joystick).to receive(:axis).with(8).and_return(-40000)
+            @joystick.read_configuration('baa')
+            value = @joystick.read_axis(8)
+            value.should == -30
+        end
+
+        it "should invert values if requested" do
+            config = {
+                :type => "Joystick",
+                :axis => {
+                    8 =>{
+                        :action => :test,
+                        :invert => true
+                    }
+                }
+            }
+            expect(YAML).to receive(:load_file).and_return(config)
+            expect(@sdl_joystick).to receive(:axis).with(8).and_return(-32767)
+            @joystick.read_configuration('baa')
+            value = @joystick.read_axis(8)
+            value.should == 30
+        end
+
+        it "should put values in the deadzone as 0" do
+            config = {
+                :type => "Joystick",
+                :axis => {
+                    8 =>{
+                        :action => :test,
+                        :dead_zone => "-40:40"
+                    }
+                }
+            }
+            expect(YAML).to receive(:load_file).and_return(config)
+            expect(@sdl_joystick).to receive(:axis).with(8).and_return(23)
+            @joystick.read_configuration('baa')
+            value = @joystick.read_axis(8)
+            value.should == 0
+        end
+
+        it "should normalize the thrust correctly" do
+            config = {
+                :type => "Joystick",
+                :axis => {
+                    8 =>{
+                        :action => :thrust,
+                        :output_range => "0:100"
+                    }
+                }
+            }
+            expect(YAML).to receive(:load_file).and_return(config)
+            expect(@sdl_joystick).to receive(:axis).with(8).and_return(32767)
+            @joystick.read_configuration('baa')
+            value = @joystick.read_axis(8)
+            value.should == 60000
+        end
+
+        it "should throotle the change rate correctly" do
+            config = {
+                :type => "Joystick",
+                :axis => {
+                    8 => {
+                        :action => :thrust,
+                        :output_range => "0:100",
+                        :input_range => "100:200",
+                        :max_change_rate => 1,
+                        :last_poll => 0.500,
+                        :last_value => 34750
+                    }
+                }
+            }
+            expect(YAML).to receive(:load_file).and_return(config)
+
+            allow(Time).to receive(:now).and_return(1.0) # 0.5 secs after
+            expect(@sdl_joystick).to receive(:axis).with(8).and_return(140)
+            @joystick.read_configuration('baa')
+            value = @joystick.read_axis(8)
+            value.should == 34497.5
+        end
+
+        it "should not throotle the change rate when increasing thrust" do
+            config = {
+                :type => "Joystick",
+                :axis => {
+                    8 =>{
+                        :action => :thrust,
+                        :output_range => "0:100",
+                        :input_range => "0:100",
+                        :max_change_rate => 1,
+                        :last_poll => 0.500,
+                        :last_value => 50
+                    }
+                }
+            }
+            expect(YAML).to receive(:load_file).and_return(config)
+
+            allow(Time).to receive(:now).and_return(1.0) # 0.5 secs after
+            expect(@sdl_joystick).to receive(:axis).with(8).and_return(100)
+            @joystick.read_configuration('baa')
+            value = @joystick.read_axis(8)
+            value.should == 60000
+        end
+    end
+
+    describe "#normalize_thrust" do
+        it "should return values within the range expected by the CF" do
+            v = @joystick.send(:normalize_thrust, 5000,
+                               {
+                                   :start => -10000.0,
+                                   :end   => 10000.0,
+                                   :width => 20000.0
+                               },{
+                                   :start => 0.0,
+                                   :end   => 80.0,
+                                   :width => 80.0
+                               })
+            v.should == 34750
+            v.should be_an_instance_of Fixnum
+        end
+
+        it "should set values under the given range to the min" do
+            v = @joystick.send(:normalize_thrust, 0,
+                               {
+                                   :start => -10000.0,
+                                   :end   => 10000.0,
+                                   :width => 20000.to_f
+                               },{
+                                   :start => 30.0,
+                                   :end   => 80.0,
+                                   :width => 50.0
+                               })
+            v.should == 24650 # 30% of 9500-60000
+            v.should be_an_instance_of Fixnum
+        end
+
+        it "should set values over the given range to the max" do
+            v = @joystick.send(:normalize_thrust, 32767,
+                               {
+                                   :start => -10000.0,
+                                   :end   => +10000.0,
+                                   :width => 20000.to_f
+                               },{
+                                   :start => 0.0,
+                                   :end   => 80.0,
+                                   :width => 80.0
+                               })
+            v.should == 49900 # 80% of 9500-60000
+            v.should be_an_instance_of Fixnum
+        end
+    end
+
+    describe "#read button" do
+        it "should return 1 when pressed" do
+            expect(@sdl_joystick).to receive(:button).and_return(true)
+            v = @joystick.send(:read_button, 1)
+            v.should == 1
+        end
+        
+        it "should return -1 when not pressed" do
+            expect(@sdl_joystick).to receive(:button).and_return(false)
+            v = @joystick.send(:read_button, 0)
+            v.should == 0
+        end
+    end
+end

data/spec/log_spec.rb

@@ -0,0 +1,266 @@
+# -*- 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'
+
+
+describe LogTOCElement do
+    describe "#initialize" do
+        it "should initialize correctly" do
+            data = [5, 23].pack('C*') + ['hello', 'world'].pack('Z*Z*')
+            lte = LogTOCElement.new(data)
+            lte.ident.should == 5
+            lte.group.should == 'hello'
+            lte.name.should  == 'world'
+            lte.ctype.should == 'float'
+            lte.directive.should == 'e'
+            lte.access.should == 0x10
+        end
+    end
+end
+
+describe LogBlock do
+    describe "#initialize" do
+        it "should count blocks correctly" do
+            b1 = LogBlock.new([]) # 0
+            b2 = LogBlock.new([]) # 1
+            b3 = LogBlock.new([]) # 2
+            b3.ident.should == 2
+        end
+    end
+
+    describe "#unpack_log_data" do
+        it "should unpack some binary data and call the callback" do
+            result = nil
+            cb = Proc.new do |r|
+                result = r
+            end
+
+            var1 = LogConfVariable.new("var1", true, 0, 1)
+            var2 = LogConfVariable.new("var2", true, 0, 2)
+            var3 = LogConfVariable.new("var3", true, 0, 3)
+
+            variables = [var1, var2, var3]
+
+            # 1 uint8, 1 uint16 and 1 uint32
+            data = [255].pack('C') + [65535].pack('S<') + [16777215].pack('L<')
+
+            b = LogBlock.new(variables) #3
+            b.data_callback = cb
+            b.unpack_log_data(data)
+            result.should == {
+                'var1' => 255,
+                'var2' => 65535,
+                'var3' => 16777215
+            }
+        end
+    end
+end
+
+describe Log do
+    before :each do
+        @crazyflie = double("Crazyflie")
+        @queue = Queue.new
+        allow(@crazyflie).to receive(:crtp_queues).and_return({:logging =>
+                                                                  @queue})
+        allow(@crazyflie).to receive(:cache_folder).and_return(nil)
+
+        @log = Log.new(@crazyflie)
+        @logger = @log.logger
+    end
+
+    describe "#initialize" do
+        it "should intialize the Log facility" do
+            @log.toc.toc.should == {}
+            @log.log_blocks.should == {}
+        end
+    end
+
+    describe "#refresh_toc" do
+        it "send the packet and fetch the TOC from the crazyflie" do
+            port = Crazyflie::CRTP_PORTS[:logging]
+            channel = TOC_CHANNEL
+            expect(@crazyflie).to receive(:send_packet).once
+            expect(@log.toc).to receive(:fetch_from_crazyflie).with(
+                                                                    @crazyflie,
+                                                                    port,
+                                                                    @queue)
+            @log.refresh_toc()
+        end
+    end
+
+    describe "#create_log_block" do
+        it "should send the create block package for toc variables" do
+            log_conf = double("LogConf")
+
+            var1 = LogConfVariable.new("var1", true, 0, 1)
+
+            var2 = LogConfVariable.new("var2", false, 0, 2, 2)
+            expect(var2).not_to receive(:name)
+
+            var3 = LogConfVariable.new("var3", true, 0, 3)
+            expect(var3).not_to receive(:address)
+
+            variables = [var1, var2, var3]
+            log_conf = LogConf.new(variables, {:period => 200})
+
+            # It is the 4th call to logblock.new in these specs
+            expect(@logger).to receive(:debug).with("Adding block 4")
+
+            packet = CRTPPacket.new()
+            packet.modify_header(nil, CRTP_PORTS[:logging],
+                                 LOG_SETTINGS_CHANNEL)
+
+            packet.data = [Log::CMD_CREATE_BLOCK] + [4] +
+                [1] + [1] + [2] + [2,0,0,0] + [3] + [32]
+
+            toc1 = TOCElement.new({:ident => 1})
+            toc3 = TOCElement.new({:ident => 32})
+            expect(@log.toc).to receive(:[]).with('var1').and_return(toc1)
+            expect(@log.toc).to receive(:[]).with('var3').and_return(toc3)
+
+            expect(@crazyflie).to receive(:send_packet) do |pk|
+                packet.header.should == pk.header
+                packet.data.should == pk.data
+            end
+            @log.create_log_block(log_conf).should == 4
+        end
+    end
+
+    describe "#start_logging" do
+        it "should send the start logging packet" do
+            logblock = double("logblock")
+            expect(logblock).to receive(:period).and_return(30)
+            expect(logblock).to receive(:data_callback=).with(an_instance_of(Proc))
+            expect(@log.log_blocks).to receive(:[]).with(5).and_return(logblock)
+            expect(@logger).to receive(:debug)
+            expect(@crazyflie).to receive(:send_packet) do |pk|
+                pk.data.should == [CMD_START_LOGGING, 5, 30]
+            end
+            @log.start_logging(5) do |values|
+            end
+        end
+    end
+
+    describe "#stop_logging" do
+        it "should send the stop logging packet" do
+            block = double("block")
+            expect(@log.log_blocks).to receive(:[]).with(5).and_return(block)
+            expect(@logger).to receive(:debug)
+            expect(@crazyflie).to receive(:send_packet) do |pk|
+                pk.data.should == [CMD_STOP_LOGGING, 5]
+            end
+            @log.stop_logging(5)
+        end
+    end
+
+    describe "#delete_block" do
+        it "should delete the block" do
+            block = double("block")
+            expect(@crazyflie).to receive(:send_packet) do |pk|
+                pk.data.should == [CMD_DELETE_BLOCK, 5]
+            end
+            expect(@log.log_blocks).to receive(:delete).with(5).and_return({})
+
+            @log.delete_block(5)
+        end
+    end
+
+    describe "#[]" do
+        it "should return a block with the given ID" do
+            expect(@log.log_blocks).to receive(:[]).with(3)
+            @log[3]
+        end
+    end
+
+    describe "#start_packet_reader_thread" do
+        it "should start a thread and process a packet" do
+            packet = CRTPPacket.new()
+            packet.modify_header(nil, nil, 3)
+            packet.channel.should == 3 # We dont process this one
+            do_this = receive(:pop).and_return(packet).at_least(:once)
+            expect(@queue).to do_this
+            expect(@logger).to receive(:debug).at_least(:once)
+            @log.start_packet_reader_thread
+            sleep 0.1
+            @log.stop_packet_reader_thread
+        end
+    end
+
+    describe "#handle_settings_packet" do
+        # This has not many secret
+        it "should go to CMD_CREATE_BLOCK case without error" do
+            packet = CRTPPacket.new()
+            packet.data = [CMD_CREATE_BLOCK, 33, 0]
+            expect(@log).not_to receive(:puts)
+            expect(@logger).to receive(:error).with("No log entry for 33")
+            @log.send(:handle_settings_packet, packet)
+        end
+
+        it "should go to CMD_CREATE_BLOCK case with error" do
+            packet = CRTPPacket.new()
+            packet.data = [CMD_CREATE_BLOCK, 33, 3]
+            expect(@logger).to receive(:error).with("Error creating block 33: 3")
+            expect(@log.log_blocks).to receive(:[]).with(33).and_return({})
+            @log.send(:handle_settings_packet, packet)
+        end
+
+        it "should go to CMD_START_LOGGING case without error" do
+            packet = CRTPPacket.new()
+            packet.data = [CMD_START_LOGGING, 33, 0]
+            expect(@logger).to receive(:debug).with("Logging started for 33")
+            @log.send(:handle_settings_packet, packet)
+        end
+
+        it "should go to CMD_START_LOGGING case with error" do
+            packet = CRTPPacket.new()
+            packet.data = [CMD_START_LOGGING, 33, 3]
+            expect(@logger).to receive(:error).with("Error starting to log 33: 3")
+            @log.send(:handle_settings_packet, packet)
+        end
+    end
+
+    describe "#handle_logdata_packet" do
+        it "should warn if no block exist" do
+            expect(@log.log_blocks).to receive(:[]).with(33).and_return(nil)
+            m = "No entry for logdata for block 33"
+            expect(@logger).to receive(:error).with(m)
+            packet = CRTPPacket.new()
+            packet.data = [33, 44, 45, 46]
+            @log.send(:handle_logdata_packet, packet)
+        end
+
+        it "should call the unpack log data if the block exists" do
+            block =  double("Block")
+            expect(block).to receive(:unpack_log_data).with([46,47].pack('C*'))
+            expect(@log.log_blocks).to receive(:[]).with(33).and_return(block)
+            expect(@logger).not_to receive(:error)
+            packet = CRTPPacket.new()
+            packet.data = [33, 44, 45, 46, 46, 47]
+            @log.send(:handle_logdata_packet, packet)
+        end
+    end
+
+    describe "#packet_factory" do
+        it "should return a new packet on logging port and settings channel" do
+            pk = @log.send(:packet_factory)
+            pk.channel.should == LOG_SETTINGS_CHANNEL
+            pk.port.should == CRTP_PORTS[:logging]
+        end
+    end
+end