hybridgroup-crubyflie 0.1.4

data/lib/crubyflie/driver/radio_driver.rb

@@ -0,0 +1,363 @@
+# -*- 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 'exceptions'
+require 'driver/crtp_packet'
+require 'crazyradio/crazyradio'
+
+
+
+module Crubyflie
+    # Small URI class since Ruby URI < 1.9.3 gives problems parsing
+    # Crazyflie URIs
+    class CrubyflieURI
+        attr_reader :scheme, :dongle, :channel, :rate
+        # Initialize an URI
+        # @param uri_str [String] the URI
+        def initialize(uri_str)
+            @uri_str = uri_str
+            @scheme, @dongle, @channel, @rate = split()
+            if @scheme.nil? || @dongle.nil? || @channel.nil? || @rate.nil? ||
+                    @scheme != 'radio'
+                raise InvalidURIException.new('Bad URI')
+            end
+        end
+
+        # Return URI as string
+        # @return [String] a string representation of the URI
+        def to_s
+            @uri_str
+        end
+
+        # Quick, dirty uri split
+        def split
+            @uri_str.sub(':', '').sub('//','/').split('/')
+        end
+        private :split
+    end
+
+
+    # This layer takes care of connecting to the crazyradio and
+    # managing the incoming and outgoing queues. This is done
+    # by spawing a thread.
+    # It also provides the interface to scan for available crazyflies
+    # to which connect
+    class RadioDriver
+        # Currently used callbacks that can be passed to connect()
+        CALLBACKS = [:link_quality_cb,
+                     :link_error_cb]
+        # Default size for the outgoing queue
+        OUT_QUEUE_MAX_SIZE = 50
+        # Default number of retries before disconnecting
+        RETRIES_BEFORE_DISCONNECT = 20
+
+        attr_reader :uri
+        attr_reader :retries_before_disconnect, :out_queue_max_size
+
+        # Initialize the driver. Creates new empty queues.
+        def initialize()
+            @uri = nil
+            @in_queue = Queue.new()
+            @out_queue = Queue.new()
+            Thread.abort_on_exception = true
+            @radio_thread = nil
+            @callbacks = {}
+            @crazyradio = nil
+            @out_queue_max_size = nil
+            @retries_before_disconnect = nil
+            @shutdown_thread = false
+        end
+
+        # Connect to a Crazyflie in the specified URI
+        # @param uri_s [String] a radio uri like radio://<dongle>/<ch>/<rate>
+        # @param callbacks [Hash] blocks to call (see CALLBACKS contant values)
+        # @param opts [Hash] options. Currently supported
+        #                    :retries_before_disconnect (defaults to 20) and
+        #                    :out_queue_max_size (defaults to 50)
+        # @raise [CallbackMissing] when a necessary callback is not provided
+        #                          (see CALLBACKS constant values)
+        # @raise [InvalidURIException] when the URI is not a valid radio URI
+        # @raise [OpenLink] when a link is already open
+        def connect(uri_s, callbacks={}, opts={})
+            # Check if apparently there is an open link
+
+            if @crazyradio
+                m = "Active link to #{@uri.to_s}. Disconnect first"
+                raise OpenLink.new(m)
+            end
+
+            # Parse URI to initialize Crazyradio
+            # @todo: better control input. It defaults to 0
+            @uri = CrubyflieURI.new(uri_s)
+            dongle_number = @uri.dongle.to_i
+            channel = @uri.channel.to_i
+            rate = @uri.rate
+
+            # @todo this should be taken care of in crazyradio
+            case rate
+            when "250K"
+                rate = CrazyradioConstants::DR_250KPS
+            when "1M"
+                rate = CrazyradioConstants::DR_1MPS
+            when "2M"
+                rate = CrazyradioConstants::DR_2MPS
+            else
+                raise InvalidURIException.new("Bad radio rate")
+            end
+
+            # Fill in the callbacks Hash
+            CALLBACKS.each do |cb|
+                if passed_cb = callbacks[cb]
+                    @callbacks[cb] = passed_cb
+                else
+                    raise CallbackMissing.new("Callback #{cb} mandatory")
+                end
+            end
+
+            @retries_before_disconnect = opts[:retries_before_disconnect] ||
+                RETRIES_BEFORE_DISCONNECT
+            @out_queue_max_size = opts[:out_queue_max_size] ||
+                OUT_QUEUE_MAX_SIZE
+
+            # Initialize Crazyradio and run thread
+            cradio_opts = {
+                :channel => channel,
+                :data_rate => rate
+            }
+            @crazyradio = Crazyradio.factory(cradio_opts)
+            start_radio_thread()
+
+        end
+
+        # Disconnects from the crazyradio
+        # @param force [TrueClass, FalseClass]. Kill the thread right away, or
+        #                                       wait for out_queue to empty
+        def disconnect(force=nil)
+            kill_radio_thread(force)
+            @in_queue.clear()
+            @out_queue.clear()
+
+            return if !@crazyradio
+            @crazyradio.close()
+            @crazyradio = nil
+        end
+
+        # Place a packet in the outgoing queue
+        # When not connected it will do nothing
+        # @param packet [CRTPPacket] The packet to send
+        def send_packet(packet)
+            return if !@crazyradio
+            if (s = @out_queue.size) >= @out_queue_max_size
+                m = "Reached #{s} elements in outgoing queue"
+                @callbacks[:link_error_cb].call(m)
+                disconnect()
+            end
+
+            @out_queue << packet if !@shutdown_thread
+        end
+
+        # Fetch a packet from the incoming queue
+        # @return [CRTPPacket,nil] a packet from the queue,
+        #                           or nil when there is none
+        def receive_packet(non_block=true)
+            begin
+                return @in_queue.pop(non_block)
+            rescue ThreadError
+                return nil
+            end
+        end
+
+        # List available Crazyflies in the provided channels
+        # @param start [Integer] channel to start
+        # @param stop [Intenger] channel to stop
+        # @return [Array] list of channels where a Crazyflie was found
+        def scan_radio_channels(start = 0, stop = 125)
+            return @crazyradio.scan_channels(start, stop)
+        end
+        private :scan_radio_channels
+
+        # List available Crazyflies
+        # @return [Array] List of radio URIs where a crazyflie was found
+        # @raise [OpenLink] if the Crazyradio is connected already
+        def scan_interface
+            raise OpenLink.new("Cannot scan when link is open") if @crazyradio
+            begin
+                @crazyradio = Crazyradio.factory()
+                results = {}
+                @crazyradio[:arc] = 1
+                @crazyradio[:data_rate] = Crazyradio::DR_250KPS
+                results["250K"] = scan_radio_channels()
+                @crazyradio[:data_rate] = Crazyradio::DR_1MPS
+                results["1M"]   = scan_radio_channels()
+                @crazyradio[:data_rate] = Crazyradio::DR_2MPS
+                results["2M"]   = scan_radio_channels()
+
+                uris = []
+                results.each do |rate, channels|
+                    channels.each do |ch|
+                        uris << "radio://0/#{ch}/#{rate}"
+                    end
+                end
+                return uris
+            rescue USBDongleException
+                raise
+            rescue Exception
+                retries ||= 0
+                logger.error("Unknown error scanning interface: #{$!}")
+                @crazyradio.reopen()
+                retries += 1
+                if retries < 2
+                    logger.error("Retrying")
+                    sleep 0.5
+                    retry
+                end
+                return []
+            ensure
+                @crazyradio.close() if @crazyradio
+                @crazyradio = nil
+            end
+        end
+
+
+        # Get status from the crazyradio. @see Crazyradio#status
+        def get_status
+            return Crazyradio.status()
+        end
+
+
+        # Privates
+        # The body of the communication thread
+        # Sends packets and tries to read the ACK
+        # @todo it is long and ugly
+        # @todo why the heck do we care here if we need to wait? Should the
+        # crazyradio do the waiting?
+        def start_radio_thread
+            @radio_thread = Thread.new do
+                Thread.current.priority = 5
+                out_data = [0xFF]
+                retries = @retries_before_disconnect
+                should_sleep = 0
+                error = "Unknown"
+                while true do
+                    begin
+                        ack = @crazyradio.send_packet(out_data)
+                        # possible outcomes
+                        # -exception - no usb dongle?
+                        # -nil - bad comm
+                        # -AckStatus class
+                    rescue Exception
+                        error = "Error talking to Crazyradio: #{$!.to_s}"
+                        break
+                    end
+
+                    if ack.nil?
+                        error = "Dongle communication error (ack is nil)"
+                        break
+                    end
+
+                    # Set this in function of the retries
+                    quality = (10 - ack.retry_count) * 10
+                    @callbacks[:link_quality_cb].call(quality)
+
+                    # Retry if we have not reached the limit
+                    if !ack.ack
+                        retries -= 1
+                        next if retries > 0
+                        error = "Too many packets lost"
+                        break
+                    else
+                        retries = @retries_before_disconnect
+                    end
+
+                    # If there is data we queue it in incoming
+                    # Otherwise we increase should_sleep
+                    # If there is no data for more than 10 times
+                    # we will sleep 0.01s when our outgoing queue
+                    # is empty. Otherwise, we just send what we have
+                    # of the 0xFF packet
+                    data = ack.data
+                    if data.length > 0
+                        @in_queue << CRTPPacket.unpack(data)
+                        should_sleep = 0
+                    else
+                        should_sleep += 1
+                    end
+
+                    break if @shutdown_thread && @out_queue.empty?()
+
+                    begin
+                        out_packet = @out_queue.pop(true) # non-block
+                        should_sleep += 1
+                    rescue ThreadError
+                        out_packet = CRTPPacket.new(0xFF)
+                        sleep 0.01 if should_sleep >= 10
+                    end
+
+                    out_data = out_packet.pack
+                end
+                if !@shutdown_thread
+                    # If we reach here it means we are dying because of
+                    # an error. The callback will likely call disconnect, which
+                    # tries to kills us, but cannot because we are running the
+                    # callback. Therefore we set @radio_thread to nil and then
+                    # run the callback.
+                    @radio_thread = nil
+                    @callbacks[:link_error_cb].call(error)
+                end
+            end
+        end
+        private :start_radio_thread
+
+        def kill_radio_thread(force=false)
+            if @radio_thread
+                if force
+                    @radio_thread.kill()
+                else
+                    @shutdown_thread = true
+                    @radio_thread.join()
+                end
+                @radio_thread = nil
+                @shutdown_thread = false
+            end
+        end
+        private :kill_radio_thread
+
+
+
+        # def pause_radio_thread
+        #     @radio_thread.stop if @radio_thread
+        # end
+        # private :pause_radio_thread
+
+
+        # def resume_radio_thread
+        #     @radio_thread.run if @radio_thread
+        # end
+        # private :resume_radio_thread
+
+
+        # def restart_radio_thread
+        #     kill_radio_thread()
+        #     start_radio_thread()
+        # end
+        # private :restart_radio_thread
+    end
+end

data/lib/crubyflie/exceptions.rb

@@ -0,0 +1,36 @@
+# -*- 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
+    # Raised when the radio URI is invalid
+    class InvalidURIException < Exception; end
+    # Raised when an radio link is already open
+    class OpenLink < Exception; end
+    # Raised when a radio driver callback parameter is missing
+    class CallbackMissing < Exception; end
+    # Raised when no USB dongle can be found
+    class NoDongleFound < Exception; end
+    # Raised when a problem occurs with the USB dongle
+    class USBDongleException < Exception; end
+    # Raised when a problem happens in the radio driver communications thread
+    class RadioThreadException < Exception; end
+    # Expected a package but it took to long to get it
+    class WaitTimeoutException < Exception; end
+    # Raised when there is a problem initializing a joystick
+    class JoystickException < Exception; end
+end

data/lib/crubyflie/input/input_reader.rb

@@ -0,0 +1,190 @@
+# -*- 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
+
+    # This class provides functionality basic to all controllers.
+    # Specific controller classes inherit from here.
+    #
+    # To read an input we must declare axis and buttons.
+    # The axis are analog float readings (range decided by the
+    # controller) while the buttons are integer where <= 0 means not pressed
+    # and > 0 means pressed.
+    #
+    # The reading of the values is implemented by children classes.
+    #
+    # The InputReader will also apply the #INPUT_ACTIONS to a given
+    # Crazyflie. In order to do that it will go through all the
+    # read values and perform actions associated to them, like sending
+    # a setpoint, shutting down the connection or altering the calibration.
+    class InputReader
+
+        # List of current recognized actions that controllers can declare
+        INPUT_ACTIONS = [:roll, :pitch, :yaw, :thrust,
+                         :roll_inc_cal, :roll_dec_cal,
+                         :pitch_inc_cal, :pitch_dec_cal,
+                         :switch_scaled_output_mode,
+                         :switch_xmode,  :close_link]
+
+        attr_reader :axis, :buttons, :axis_readings, :button_readings
+        attr_accessor :xmode
+        # An input is composed by several necessary axis and buttons.
+        # @param axis [Hash] A hash of keys identifying axis IDs
+        #                    (the controller should know to what the
+        #                     ID maps), and values from #INPUT_ACTIONS
+        # @param buttons [Hash] A hash of keys identifying button IDs (the
+        #                    controller should know to what the ID maps),
+        #                    and values from #INPUT_ACTIONS
+        def initialize(axis, buttons)
+            @axis = axis
+            @buttons = buttons
+            @calibrations = {}
+            @xmode = false
+            @output_scale = 0 # off
+
+            # Calibrate defaults to 0
+            INPUT_ACTIONS.each do |action|
+                @calibrations[action] = 0
+            end
+
+            @axis_readings = {}
+            @button_readings = {}
+        end
+
+        # Read inputs will call read_axis() on all the declared axis
+        # and read_button() on all the declared buttons.
+        # After obtaining the reading, it will apply calibrations to
+        # the result. Apply the read values with #apply_input
+        def read_input
+            poll() # In case we need to poll the device
+            actions_to_axis = @axis.invert()
+            actions_to_axis.each do |action, axis_id|
+                if !INPUT_ACTIONS.include?(action)
+                    logger.error("Unknown action #{action}. Skipping")
+                    next
+                end
+                @axis_readings[action] = read_axis(axis_id)
+                @axis_readings[action] += @calibrations[action]
+            end
+
+            actions_to_buttons = @buttons.invert()
+            actions_to_buttons.each do |action, button_id|
+                if !INPUT_ACTIONS.include?(action)
+                    logger.error("Unknown action #{action}. Skipping")
+                    next
+                end
+                @button_readings[action] = read_button(button_id)
+                @button_readings[action] += @calibrations[action]
+            end
+        end
+
+        # This will act on current axis readings (by sendint a setpoint to
+        # the crazyflie) and on button readings (by, for example, shutting
+        # down the link or modifying the calibrations).
+        # If the link to the crazyflie is down, it will not send anything.
+        # @param crazyflie [Crazyflie] A crazyflie instance to send the
+        #                              setpoint to.
+        def apply_input(crazyflie)
+            return if !crazyflie.active?
+            setpoint = {
+                :roll => nil,
+                :pitch => nil,
+                :yaw => nil,
+                :thrust => nil
+            }
+
+            @button_readings.each do |action, value|
+                case action
+                when :roll
+                    setpoint[:roll] = value
+                when :pitch
+                    setpoint[:pitch] = value
+                when :yaw
+                    setpoint[:yaw] = value
+                when :thrust
+                    setpoint[:thrust] = value
+                when :roll_inc_cal
+                    @calibrations[:roll] += 1
+                when :roll_dec_cal
+                    @calibrations[:roll] -= 1
+                when :pitch_inc_cal
+                    @calibrations[:pitch] += 1
+                when :pitch_dec_cal
+                    @calibrations[:pitch] -= 1
+                when :switch_xmode
+                    @xmode = !@xmode if value > 0
+                    logger.info("Xmode is #{@xmode}") if value > 0
+                when :switch_scaled_output_mode
+                    if value > 0 && @output_scale == 0
+                        logger.info("Scaling output: x#{value}")
+                        @output_scale = value.to_f
+                    elsif value > 0 && @output_scale > 0
+                        logger.info("Scaling output disabled")
+                        @output_scale = 0
+                    end
+                when :close_link
+                    crazyflie.close_link() if value > 0
+                end
+            end
+
+            return if !crazyflie.active?
+
+            @axis_readings.each do |action, value|
+                case action
+                when :roll
+                    setpoint[:roll] = value
+                when :pitch
+                    setpoint[:pitch] = value
+                when :yaw
+                    setpoint[:yaw] = value
+                when :thrust
+                    setpoint[:thrust] = value
+                end
+            end
+
+            setpoint.keys().each do |k|
+                next if k == :thrust
+                setpoint[k] *= @output_scale
+            end if @output_scale > 0
+
+            pitch  = setpoint[:pitch]
+            roll   = setpoint[:roll]
+            yaw    = setpoint[:yaw]
+            thrust = setpoint[:thrust]
+
+            if pitch && roll && yaw && thrust
+                m = "Sending R: #{roll} P: #{pitch} Y: #{yaw} T: #{thrust}"
+                logger.debug(m)
+                crazyflie.commander.send_setpoint(roll, pitch, yaw, thrust,
+                                                  @xmode)
+            end
+        end
+
+        private
+        def read_axis(axis_id)
+            raise Exception.new("Not implemented!")
+        end
+
+        def read_button(button_id)
+            raise Exception.new("Not implemented!")
+        end
+
+        def poll
+        end
+    end
+end