Add hovering support via the flightmode.althold parameter

This should work with latest version of the copter's firmware and crazyradio

.travis.yml

@@ -3,6 +3,8 @@ rvm:
   - 1.9.2
   - 1.9.3
   - 2.0.0
+  - 2.1.6
+  - 2.2.2
 before_install:
  - sudo apt-get update -qq
- -  sudo apt-get install -qq libsdl1.2-dev
+ - sudo apt-get install -qq libsdl1.2-dev

README.md

@@ -1,37 +1,34 @@
-Crubyflie - A Ruby client for Crazyflie
-=======================================
+Crubyflie - A Ruby client for Crazyflie 1
+=========================================
 
-[![Build Status](https://travis-ci.org/hsanjuan/crubyflie.png?branch=master)](https://travis-ci.org/hsanjuan/crubyflie) [![Coverage Status](https://coveralls.io/repos/hsanjuan/crubyflie/badge.png)](https://coveralls.io/r/hsanjuan/crubyflie)
+[![Gem Version](https://badge.fury.io/rb/crubyflie.svg)](http://badge.fury.io/rb/crubyflie) [![Build Status](https://travis-ci.org/hsanjuan/crubyflie.png?branch=master)](https://travis-ci.org/hsanjuan/crubyflie) [![Coverage Status](https://coveralls.io/repos/hsanjuan/crubyflie/badge.png)](https://coveralls.io/r/hsanjuan/crubyflie)
 
-Crubyflie is a Ruby rewrite of the [Crazyflie quadcopter](http://www.bitcraze.se/category/crazyflie/) Python [client libraries](https://bitbucket.org/bitcraze/crazyflie-pc-client), with some customizations.
+Crubyflie is a Ruby rewrite of the [Crazyflie 1.0 quadcopter](http://www.bitcraze.se/category/crazyflie/) Python [client libraries](https://github.com/bitcraze/crazyflie-clients-python), with some customizations.
 
-The Crazyflie is awesome, but I did not know where to start contributing. Therefore I thought that rewriting the code in Ruby would be one way of knowing what is going on and how it works. Along the way I took the time to document all the code so that others can understand it and create tests.
+The Crazyflie is awesome, but I did not know where to start contributing. Therefore I thought that rewriting the code in Ruby would be one way of knowing what is going on and how it works. Along the way I took the time to document all the code so that others can understand it.
 
 You may be also interested in some other unofficial Crazyflie clients:
 
  * C++:     https://github.com/fairlight1337/libcflie
  * Node.js: https://github.com/ceejbot/aerogel
  * Haskell: https://github.com/orclev/crazyflie-haskell
 
-Disclaimer
-----------
-
-Crubyflie is in early stage of development, very untested.
-
 Features
 --------
 
  * Crubyflie can be used to fly a Crazyflie device using a Joystick and the Crazyradio USB dongle
  * Crubyflie exposes an API that allows to control the copter, read logging, parameters and console easily
- * Crubyflie runs headless  
+ * Crubyflie runs headless
  * Lightweight: If you just want to fly, Crubyflie consumes around 1/2 memory and 1/3 CPU compared to the original Python `cfheadless` utility.
+ * Hovering mode (see requirements below)
+
+Requirements
+------------
+
+Crubyflie versions `>= 0.2.0` support hovering mode and are compatible with the latest firmware of `Crazyradio` ([Version 0.53](https://github.com/bitcraze/crazyradio-firmware/releases/tag/0.53)) and `Crazyflie` ([Version 2015.1](https://github.com/bitcraze/crazyflie-firmware/releases/tag/2015.1))
+
+Old versions should probably work, otherwise you can try with the `0.1.3` gem version.
 
-Not included...
-----------------
- * No fancy UI.
- * No flash utility (yet?).
- * No idea how this works in other OSs that are not Linux, but in theory it should work in all with some small fixes. I welcome you to take on this task if you are interested.
- * No support for Ruby <= 1.8.7 (maybe it works who knows... I haven't tested but since Crubyflie relies heavily on threading probably it does not work so good).
 
 Installation
 ------------
@@ -42,12 +39,12 @@ Crubyflie depends on `rubysdl`, for which you will need the SDL library and head
 
 That's all.
 
-Fyling the Crazyflie
+Flying the Crazyflie
 --------------------
 
 The easiest way to do it is to `gem install crubyflie` and then run the `crubyflie` command. This will connect to the first visible quadcopter using the first available joystick on your computer (you can modify this parameters with the appropiate flags):
 
-    > crubyflie2.0 -h
+    > crubyflie -h
     Options:
       --joystick-id, -j <i>:   Joystick ID (default: 0)
            --cf-uri, -f <s>:   Crazyflie URI (defaults to first one found in scan)
@@ -96,15 +93,15 @@ end
 
 
 # Interface to the param facility
-@cf.param.get_value(...) do |value|
+@cf.param.get_value('param1.name') do |value|
   ...
 end
 
-@cf.param.get_value(...) do |value|
+@cf.param.get_value('param2.name') do |value|
   ...
 end
 
-@cf.param.set_value(...)
+@cf.param.set_value('param.name', 1)
 
 # Interface to the commander facility
 @cf.commander.send_setpoint(...)
@@ -115,10 +112,11 @@ end
 end
 ```
 
-That's pretty much all. As you see, instead of declaring callbacks, registering them etc. We let the user pass blocks, which are run when the data is available. 
+That's pretty much all. As you see, instead of declaring callbacks, registering them etc. We let the user pass blocks, which are run when the data is available.
+
 In Crubyflie, params are read and set synchronously, while the block passed to `start_logging()` will be called repeteadly and asynchrnously until `stop_logging()` is invoked. Console offers both synchronous `read()` and asynchronous `start_reading()` options.
 
-There are some examples in the `examples` folder. Read the gem documentation to get full information of the parameters for each function call.
+**There are some examples in the `examples` folder**. Read the gem documentation to get full information of the parameters for each function call.
 
 
 Contributing

bin/crubyflie

@@ -17,6 +17,8 @@
 # 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__), '..', 'lib')
+
 require 'crubyflie'
 require 'trollop'
 

configs/joystick_default.yaml

@@ -42,6 +42,7 @@
 # :pitch - Controls pitch. Assignable a axis or a button with value tag.
 # :thrust - Controls thrust. Assignable a axis or a button with value tag.
 # :yaw - Controls yaw. Assignable a axis or a button with value tag.
+# :hover - Enables/disables hovering mode
 # :switch_xmode - Enables/disables xmode. Assignable to a button.
 # :close_link - Kills the link and shuts Crubyflie. Assignable to a button.
 # :switch_scaled_output_mode - Enables/disabled scaled output mode. Assignable
@@ -103,4 +104,6 @@
     # When enabled, this mode will will multiply axis readings (except thrust)
     # by the value of the button (must be a positive number)
     :action: :switch_scaled_output_mode
-    :value: 0.50 # Softer output, useful for landing
+    :value: 0.50 # Softer output, useful for landing
+  5:
+    :action: :hover

examples/params_and_logging.rb

@@ -44,13 +44,13 @@
 
 # Read some parameters
 puts "--------"
-cf.param.get_value("attitudepid.kp_pitch") do |value|
+cf.param.get_value("pid_attitude.pitch_kp") do |value|
     puts "kp_pitch: #{value}"
 end
-cf.param.get_value("attitudepid.ki_pitch") do |value|
+cf.param.get_value("pid_attitude.pitch_ki") do |value|
     puts "ki_pitch: #{value}"
 end
-cf.param.get_value("attitudepid.kd_pitch") do |value|
+cf.param.get_value("pid_attitude.pitch_kd") do |value|
     puts "kd_pitch: #{value}"
 end
 puts "--------"

lib/crubyflie/crazyflie.rb

@@ -106,7 +106,7 @@ def open_link(uri)
                 sleep 0.5 # Allow setup and failures
                 setup_connection() if @link
             rescue Exception
-                #logger.warn $!.backtrace.join("\n")
+                # logger.warn $!.backtrace.join("\n")
                 call_cb(:connection_failed, $!.message)
                 close_link()
             end

lib/crubyflie/driver/radio_driver.rb

@@ -67,7 +67,7 @@ class RadioDriver
         # Default size for the outgoing queue
         OUT_QUEUE_MAX_SIZE = 50
         # Default number of retries before disconnecting
-        RETRIES_BEFORE_DISCONNECT = 20
+        RETRIES_BEFORE_DISCONNECT = 10
 
         attr_reader :uri
         attr_reader :retries_before_disconnect, :out_queue_max_size

lib/crubyflie/input/input_reader.rb

@@ -39,7 +39,7 @@ class InputReader
                          :roll_inc_cal, :roll_dec_cal,
                          :pitch_inc_cal, :pitch_dec_cal,
                          :switch_scaled_output_mode,
-                         :switch_xmode,  :close_link]
+                         :switch_xmode, :hover, :close_link]
 
         attr_reader :axis, :buttons, :axis_readings, :button_readings
         attr_accessor :xmode
@@ -55,6 +55,7 @@ def initialize(axis, buttons)
             @buttons = buttons
             @calibrations = {}
             @xmode = false
+            @hover = false
             @output_scale = 0 # off
 
             # Calibrate defaults to 0
@@ -105,9 +106,12 @@ def apply_input(crazyflie)
                 :roll => nil,
                 :pitch => nil,
                 :yaw => nil,
-                :thrust => nil
+                :thrust => nil,
+                :hover => 0
             }
 
+            set_althold = false
+
             @button_readings.each do |action, value|
                 case action
                 when :roll
@@ -118,6 +122,12 @@ def apply_input(crazyflie)
                     setpoint[:yaw] = value
                 when :thrust
                     setpoint[:thrust] = value
+                when :hover
+                    if value > 0
+                        @hover = !@hover
+                        logger.info("Hover is #{@hover}")
+                        set_althold = true
+                    end
                 when :roll_inc_cal
                     @calibrations[:roll] += 1
                 when :roll_dec_cal
@@ -173,6 +183,9 @@ def apply_input(crazyflie)
                 crazyflie.commander.send_setpoint(roll, pitch, yaw, thrust,
                                                   @xmode)
             end
+            if set_althold
+                crazyflie.param.set_value('flightmode.althold', @hover ? 1 : 0)
+            end
         end
 
         private

lib/crubyflie/input/joystick_input_reader.rb

@@ -43,6 +43,7 @@ class Joystick < InputReader
                                         "configs", "joystick_default.yaml")
         THRUST_MAX = 60000
         THRUST_MIN = 9500
+        THRUST_IDLE = 32767 # for hovering
 
         attr_reader :config, :joystick_index
         # Initializes the Joystick configuration and the SDL library
@@ -226,8 +227,12 @@ def read_axis(axis_id)
             # 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)
+            # Rate limitation applies to all inputs except thrust
+            # Thrust is only affected when no hovering and decreasing
+            # except thrust+hovering
+            # and thrust increase (needs to be quick)
+            if !is_thrust ||
+                    (is_thrust && !@hover && 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
@@ -249,23 +254,34 @@ def read_axis(axis_id)
         # Returns integer from 9.500 to 60.000 which is what the crazyflie
         # expects
         def normalize_thrust(value, input_range, output_range)
-            value = 0 if value < 0
+            # Yes, we now can have negative values for althold mode
+            value = 0 if value < 0 if !@hover
+            if @hover && value == 0
+                return THRUST_IDLE
+            end
+            # in any other case,
+            # first normalize to -100->100
+            # and then if not @hovering, put the value in users
+            # output-range (which is 0 to 100%). Finally,
+            # normalize to Crazyflie THRUST_MIN -> MAX
             range = {
                 :start => -100.0,
                 :end => 100.0,
                 :width => 200.0
             }
             value = normalize(value, input_range, range)
 
-            if value > output_range[:end] then value = output_range[:end]
-            elsif value < output_range[:start] then value = output_range[:start]
-            end
+            if !@hover
+                if value > output_range[:end] then value = output_range[:end]
+                elsif value < output_range[:start] then value = output_range[:start]
+                end
 
-            range = {
-                :start => 0.0,
-                :end => 100.0,
-                :width => 100.0
-            }
+                range = {
+                    :start => 0.0,
+                    :end => 100.0,
+                    :width => 100.0
+                }
+            end
 
             cf_range = {
                 :start => THRUST_MIN,

lib/crubyflie/version.rb

@@ -18,5 +18,5 @@
 
 module Crubyflie
     # Current gem version
-    VERSION = "0.1.3"
+    VERSION = "0.2.0"
 end