Class: Crubyflie::Joystick

Inherits:
InputReader show all
Includes:
Logging
Defined in:
lib/crubyflie/input/joystick_input_reader.rb

Overview

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

Constant Summary collapse

CONFIG_TYPE =

Configuration type for Joystick configuration

"Joystick"
DEFAULT_INPUT_RANGE =

Default SDL joystick input range for axis

"-32768:32767"
DEFAULT_OUTPUT_RANGE =

Default Crazyflie min/max angles in degrees

"-30:30"
DEFAULT_DEAD_ZONE =

Default dead zone range

"0:0"
DEFAULT_CONFIG_PATH =

Default configuration file

File.join(File.dirname(__FILE__), "..","..","..",
"configs", "joystick_default.yaml")
THRUST_MAX = 
60000
THRUST_MIN = 
9500

Constants inherited from InputReader

InputReader::INPUT_ACTIONS

Instance Attribute Summary collapse

Attributes inherited from InputReader

#axis, #axis_readings, #button_readings, #buttons, #xmode

Instance Method Summary collapse

Methods included from Logging

logger, #logger, #logger=

Methods inherited from InputReader

#apply_input, #read_input

Constructor Details

#initialize(config_path = DEFAULT_CONFIG_PATH, joystick_index = 0) ⇒ Joystick

Initializes the Joystick configuration and the SDL library leaving things ready to read values

Parameters:

  • config_path (String) (defaults to: DEFAULT_CONFIG_PATH) —

    path to configuration file

  • joystick_index (Integer) (defaults to: 0) —

    the index of the joystick in SDL



52
53
54
55
56
57
58
59
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 52

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

Instance Attribute Details

#config ⇒ Object (readonly)

Returns the value of attribute config.



47
48
49
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 47

def config
  @config
end

#joystick_index ⇒ Object (readonly)

Returns the value of attribute joystick_index.



47
48
49
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 47

def joystick_index
  @joystick_index
end

Instance Method Details

#init_sdl ⇒ Object Also known as: init

Init SDL and open the joystick

Raises:



148
149
150
151
152
153
154
155
156
157
158
159
160
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 148

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

#normalize(value, from_range, to_range) ⇒ Float

Linear-transforms a value in one range to a different range

Parameters:

  • value (Fixnum, Float) —

    the value in the original range

  • from_range (Hash) —

    the range from which we want to normalize. a range must have :start, :end, :width keys

  • to_range (Hash) —

    the destination range

Returns:

  • (Float) —

    the linear-corresponding value in the destination range



318
319
320
321
322
323
324
325
326
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 318

def normalize(value, from_range, to_range)
    from_min = from_range[:start]
    to_min = to_range[:start]
    to_w = to_range[:width]
    from_w = from_range[:width]
    # 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(2)
end

#quit ⇒ Object

Closes the opened resources in SDL



62
63
64
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 62

def quit
    SDL.quit()
end

#read_axis(axis_id) ⇒ Fixnum, Float

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.

Parameters:

  • axis_id (Integer) —

    The SDL joystick axis to be read

Returns:

  • (Fixnum, Float) —

    the correctly-normalized-value from the axis



174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 174

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]
    last_value = axis_conf[:last_value]
    invert = axis_conf[:invert]
    calibration = axis_conf[:calibration]

    input_range = axis_conf[:input_range]
    output_range = axis_conf[:output_range]

    max_chrate = axis_conf[:max_change_rate]

    dead_zone = axis_conf[:dead_zone]

    value = @joystick.axis(axis_id)

    value *= -1 if invert
    value += calibration


    # Make sure input falls with the expected range and take care of
    # the dead zone
    if dead_zone[:start] < value && dead_zone[:end] > value
        value = 0
    elsif dead_zone[:start] >= value
        value = value - dead_zone[:start]
    elsif dead_zone[:end] <= value
        value = value - dead_zone[:end]
    end

    if value > input_range[:end]
        value = input_range[:end]
    elsif value < input_range[:start]
        value = input_range[:start]
    end

    # Convert
    if is_thrust
        value = normalize_thrust(value, input_range, output_range)
    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 * 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

#read_configuration(path) ⇒ Array[Hash]

Parses a YAML Configuration files

Parameters:

  • path (String) —

    Path to the file

Returns:

  • (Array[Hash]) —

    an array with axis and buttons and and their associated action

Raises:



71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 71

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

        # Parse and fill in ranging values
        [[:input_range, DEFAULT_INPUT_RANGE],
         [:output_range, DEFAULT_OUTPUT_RANGE],
         [:dead_zone, DEFAULT_DEAD_ZONE]].each do |id, default|
            range_s = axis_cfg[id] || default
            start, rend = range_s.split(':')
            start = start.to_i; rend = rend.to_i
            range = {
                :start => start.to_f,
                :end => rend.to_f,
                :width => (Range.new(start,rend).to_a.size() - 1).to_f
            }
            axis_cfg[id] = range
        end

        # output value max jump per second. We covert to rate/ms
        max_chrate = axis_cfg[:max_change_rate] || 10000
        if action == :thrust
            # Thrust expressed in %
            w = THRUST_MAX - THRUST_MIN
            max_chrate = (max_chrate.to_f * w /100) / 1000
        else
            max_chrate = max_chrate.to_f / 1000
        end
        axis_cfg[:max_change_rate] = max_chrate

        axis_cfg[:last_poll] ||= 0
        axis_cfg[:last_value] ||= 0
        axis_cfg[:invert] ||= false
        axis_cfg[:calibration] ||= 0

    end

    buttons = {}
    config_h[:buttons] = {} if config_h[:buttons].nil?

    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
        button_cfg[:value] ||= 1
    end

    @config = config_h

    #logger.info "Loaded configuration correctly (#{path})"
    return axis, buttons
end