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_CONFIG_PATH =

Default configuration file

File.join(File.dirname(__FILE__), "..","..","..",
"configs", "joystick_default.yaml")

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



48
49
50
51
52
53
54
55
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 48

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.



43
44
45
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 43

def config
  @config
end

#joystick_index ⇒ Object (readonly)

Returns the value of attribute joystick_index.



43
44
45
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 43

def joystick_index
  @joystick_index
end

Instance Method Details

#init_sdl ⇒ Object Also known as: init

Init SDL and open the joystick

Raises:



112
113
114
115
116
117
118
119
120
121
122
123
124
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 112

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 (Range) —

    the range from which we want to normalize

  • to_range (Range) —

    the destination range

Returns:

  • (Float) —

    the linear-corresponding value in the destination range



270
271
272
273
274
275
276
277
278
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 270

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

#quit ⇒ Object

Closes the opened resources in SDL



58
59
60
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 58

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



138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
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
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 138

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

#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:



67
68
69
70
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
 
# File 'lib/crubyflie/input/joystick_input_reader.rb', line 67

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