class Crubyflie::Joystick
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
Constants
- CONFIG_TYPE
Configuration type for
Joystick
configuration- DEFAULT_CONFIG_PATH
Default configuration file
- DEFAULT_DEAD_ZONE
Default dead zone range
- DEFAULT_INPUT_RANGE
Default SDL joystick input range for axis
- DEFAULT_OUTPUT_RANGE
Default
Crazyflie
min/max angles in degrees- THRUST_IDLE
- THRUST_MAX
- THRUST_MIN
Attributes
Public Class Methods
Initializes the Joystick
configuration and the SDL library leaving things ready to read values @param config_path [String] path to configuration file @param joystick_index
[Integer] the index of the joystick in SDL
Crubyflie::InputReader::new
# 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
Public Instance Methods
Init SDL and open the joystick @raise [JoystickException] if the joystick index is plainly wrong
# 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
Linear-transforms a value in one range to a different range @param value [Fixnum, Float] the value in the original range @param from_range [Hash] the range from which we want to normalize.
a range must have :start, :end, :width keys
@param to_range [Hash] the destination range @return [Float] the linear-corresponding value in the destination
range
# File lib/crubyflie/input/joystick_input_reader.rb, line 333 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
Closes the opened resources in SDL
# File lib/crubyflie/input/joystick_input_reader.rb, line 62 def quit SDL.quit() end
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. @param axis_id [Integer] The SDL joystick axis to be read @return [Fixnum, Float] the correctly-normalized-value from the axis
# 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 # 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 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
Parses a YAML Configuration files @param path [String] Path to the file @return [Array] an array with axis and buttons and
and their associated action
@raise [JoystickException] on several configuration error cases
# 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
Private Instance Methods
Returns integer from 9.500 to 60.000 which is what the crazyflie expects
# File lib/crubyflie/input/joystick_input_reader.rb, line 255 def normalize_thrust(value, input_range, output_range) # 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 !@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 } end cf_range = { :start => THRUST_MIN, :end => THRUST_MAX, :width => THRUST_MAX - THRUST_MIN } return normalize(value, range, cf_range).round end
Called before reading
# File lib/crubyflie/input/joystick_input_reader.rb, line 321 def poll SDL::Joystick.update_all end