crubyflie 0.1.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- checksums.yaml +7 -0
- data/.gitignore +18 -0
- data/Gemfile +4 -0
- data/LICENSE.txt +674 -0
- data/README.md +99 -0
- data/Rakefile +15 -0
- data/bin/crubyflie +85 -0
- data/configs/joystick_default.yaml +48 -0
- data/crubyflie.gemspec +50 -0
- data/examples/params_and_logging.rb +87 -0
- data/lib/crubyflie/crazyflie/commander.rb +54 -0
- data/lib/crubyflie/crazyflie/console.rb +67 -0
- data/lib/crubyflie/crazyflie/log.rb +383 -0
- data/lib/crubyflie/crazyflie/log_conf.rb +57 -0
- data/lib/crubyflie/crazyflie/param.rb +220 -0
- data/lib/crubyflie/crazyflie/toc.rb +239 -0
- data/lib/crubyflie/crazyflie/toc_cache.rb +87 -0
- data/lib/crubyflie/crazyflie.rb +282 -0
- data/lib/crubyflie/crazyradio/crazyradio.rb +301 -0
- data/lib/crubyflie/crazyradio/radio_ack.rb +48 -0
- data/lib/crubyflie/crubyflie_logger.rb +74 -0
- data/lib/crubyflie/driver/crtp_packet.rb +146 -0
- data/lib/crubyflie/driver/radio_driver.rb +333 -0
- data/lib/crubyflie/exceptions.rb +36 -0
- data/lib/crubyflie/input/input_reader.rb +168 -0
- data/lib/crubyflie/input/joystick_input_reader.rb +280 -0
- data/lib/crubyflie/version.rb +22 -0
- data/lib/crubyflie.rb +31 -0
- data/spec/commander_spec.rb +67 -0
- data/spec/console_spec.rb +76 -0
- data/spec/crazyflie_spec.rb +176 -0
- data/spec/crazyradio_spec.rb +226 -0
- data/spec/crtp_packet_spec.rb +79 -0
- data/spec/crubyflie_logger_spec.rb +39 -0
- data/spec/crubyflie_spec.rb +20 -0
- data/spec/input_reader_spec.rb +136 -0
- data/spec/joystick_cfg.yaml +48 -0
- data/spec/joystick_input_reader_spec.rb +238 -0
- data/spec/log_spec.rb +266 -0
- data/spec/param_spec.rb +166 -0
- data/spec/radio_ack_spec.rb +43 -0
- data/spec/radio_driver_spec.rb +227 -0
- data/spec/spec_helper.rb +51 -0
- data/spec/toc_cache_spec.rb +87 -0
- data/spec/toc_spec.rb +187 -0
- data/tools/sdl-joystick-axis.rb +69 -0
- metadata +222 -0
@@ -0,0 +1,280 @@
|
|
1
|
+
# -*- coding: utf-8 -*-
|
2
|
+
# Copyright (C) 2013 Hector Sanjuan
|
3
|
+
|
4
|
+
# This file is part of Crubyflie.
|
5
|
+
|
6
|
+
# Crubyflie is free software: you can redistribute it and/or modify
|
7
|
+
# it under the terms of the GNU General Public License as published by
|
8
|
+
# the Free Software Foundation, either version 3 of the License, or
|
9
|
+
# (at your option) any later version.
|
10
|
+
|
11
|
+
# Crubyflie is distributed in the hope that it will be useful,
|
12
|
+
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
13
|
+
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
14
|
+
# GNU General Public License for more details.
|
15
|
+
|
16
|
+
# You should have received a copy of the GNU General Public License
|
17
|
+
# along with Crubyflie. If not, see <http://www.gnu.org/licenses/>
|
18
|
+
|
19
|
+
require 'rubygems'
|
20
|
+
require 'yaml'
|
21
|
+
require 'sdl'
|
22
|
+
|
23
|
+
require 'input/input_reader'
|
24
|
+
|
25
|
+
module Crubyflie
|
26
|
+
|
27
|
+
# Reads Joystick configuration and specific joystick input
|
28
|
+
# See the default Joystick configuration file in the configs/
|
29
|
+
# folder to have an idea what a configuration file looks like
|
30
|
+
class Joystick < InputReader
|
31
|
+
include Logging
|
32
|
+
|
33
|
+
# Configuration type for Joystick configuration
|
34
|
+
CONFIG_TYPE = "Joystick"
|
35
|
+
# Default SDL joystick input range for axis
|
36
|
+
DEFAULT_INPUT_RANGE = "-32768:32767"
|
37
|
+
# Default Crazyflie min/max angles in degrees
|
38
|
+
DEFAULT_OUTPUT_RANGE = "-30:30"
|
39
|
+
# Default configuration file
|
40
|
+
DEFAULT_CONFIG_PATH = File.join(File.dirname(__FILE__), "..","..","..",
|
41
|
+
"configs", "joystick_default.yaml")
|
42
|
+
|
43
|
+
attr_reader :config, :joystick_index
|
44
|
+
# Initializes the Joystick configuration and the SDL library
|
45
|
+
# leaving things ready to read values
|
46
|
+
# @param config_path [String] path to configuration file
|
47
|
+
# @param joystick_index [Integer] the index of the joystick in SDL
|
48
|
+
def initialize(config_path=DEFAULT_CONFIG_PATH, joystick_index = 0)
|
49
|
+
@config = nil
|
50
|
+
@joystick_index = joystick_index
|
51
|
+
@joystick = nil
|
52
|
+
axis, buttons = read_configuration(config_path)
|
53
|
+
super(axis, buttons)
|
54
|
+
|
55
|
+
end
|
56
|
+
|
57
|
+
# Closes the opened resources in SDL
|
58
|
+
def quit
|
59
|
+
SDL.quit()
|
60
|
+
end
|
61
|
+
|
62
|
+
# Parses a YAML Configuration files
|
63
|
+
# @param path [String] Path to the file
|
64
|
+
# @return [Array[Hash]] an array with axis and buttons and
|
65
|
+
# and their associated action
|
66
|
+
# @raise [JoystickException] on several configuration error cases
|
67
|
+
def read_configuration(path)
|
68
|
+
begin
|
69
|
+
config_h = YAML.load_file(path)
|
70
|
+
rescue
|
71
|
+
raise JoystickException.new("Could load YAML: #{$!}")
|
72
|
+
end
|
73
|
+
|
74
|
+
if config_h[:type] != CONFIG_TYPE
|
75
|
+
m = "Configuration is not of type #{CONFIG_TYPE}"
|
76
|
+
raise JoystickException.new(m)
|
77
|
+
end
|
78
|
+
|
79
|
+
axis = {}
|
80
|
+
if config_h[:axis].nil?
|
81
|
+
raise JoystickException.new("No axis section")
|
82
|
+
end
|
83
|
+
config_h[:axis].each do |id, axis_cfg|
|
84
|
+
action = axis_cfg[:action]
|
85
|
+
if action.nil?
|
86
|
+
raise JoystickException.new("Axis #{id} needs an action")
|
87
|
+
end
|
88
|
+
|
89
|
+
axis[id] = action
|
90
|
+
end
|
91
|
+
|
92
|
+
buttons = {}
|
93
|
+
if config_h[:buttons].nil?
|
94
|
+
raise JoystickException.new("No buttons section")
|
95
|
+
end
|
96
|
+
config_h[:buttons].each do |id, button_cfg|
|
97
|
+
action = button_cfg[:action]
|
98
|
+
if action.nil?
|
99
|
+
raise JoystickException.new("Button #{id} needs an action")
|
100
|
+
end
|
101
|
+
buttons[id] = action
|
102
|
+
end
|
103
|
+
|
104
|
+
@config = config_h
|
105
|
+
|
106
|
+
#logger.info "Loaded configuration correctly (#{path})"
|
107
|
+
return axis, buttons
|
108
|
+
end
|
109
|
+
|
110
|
+
# Init SDL and open the joystick
|
111
|
+
# @raise [JoystickException] if the joystick index is plainly wrong
|
112
|
+
def init_sdl
|
113
|
+
SDL.init(SDL::INIT_JOYSTICK)
|
114
|
+
SDL::Joystick.poll = false
|
115
|
+
n_joy = SDL::Joystick.num
|
116
|
+
logger.info("Joysticks found: #{n_joy}")
|
117
|
+
|
118
|
+
if @joystick_index >= n_joy
|
119
|
+
raise JoystickException.new("No valid joystick index")
|
120
|
+
end
|
121
|
+
@joystick = SDL::Joystick.open(@joystick_index)
|
122
|
+
name = SDL::Joystick.index_name(@joystick_index)
|
123
|
+
logger.info("Using Joystick: #{name}")
|
124
|
+
end
|
125
|
+
alias_method :init, :init_sdl
|
126
|
+
|
127
|
+
# Used to read the current state of an axis. This is a rather
|
128
|
+
# complicated operation. Raw value is first fit withing the input
|
129
|
+
# range limits, then set to 0 if it falls in the dead zone,
|
130
|
+
# then normalized to the output range that we will like to get (with
|
131
|
+
# special case for thrust, as ranges have different limits), then
|
132
|
+
# we check if the new value falls withing the change rate limit
|
133
|
+
# and modify it if not, finally we re-normalize the thrust if needed
|
134
|
+
# and return the reading, which should be good to be fit straight
|
135
|
+
# into the Crazyflie commander.
|
136
|
+
# @param axis_id [Integer] The SDL joystick axis to be read
|
137
|
+
# @return [Fixnum, Float] the correctly-normalized-value from the axis
|
138
|
+
def read_axis(axis_id)
|
139
|
+
return 0 if !@joystick
|
140
|
+
axis_conf = @config[:axis][axis_id]
|
141
|
+
return 0 if axis_conf.nil?
|
142
|
+
is_thrust = axis_conf[:action] == :thrust
|
143
|
+
|
144
|
+
|
145
|
+
last_poll = axis_conf[:last_poll] || 0
|
146
|
+
last_value = axis_conf[:last_value] || 0
|
147
|
+
invert = axis_conf[:invert] || false
|
148
|
+
calibration = axis_conf[:calibration] || 0
|
149
|
+
|
150
|
+
input_range_s = axis_conf[:input_range] || DEFAULT_INPUT_RANGE
|
151
|
+
ir_start, ir_end = input_range_s.split(':')
|
152
|
+
input_range = Range.new(ir_start.to_i, ir_end.to_i)
|
153
|
+
|
154
|
+
output_range_s = axis_conf[:output_range] || DEFAULT_OUTPUT_RANGE
|
155
|
+
or_start, or_end = output_range_s.split(':')
|
156
|
+
output_range = Range.new(or_start.to_i, or_end.to_i)
|
157
|
+
|
158
|
+
# output value max jump per second. We covert to rate/ms
|
159
|
+
max_chrate = axis_conf[:max_change_rate] || 10000
|
160
|
+
max_chrate = max_chrate.to_f / 1000
|
161
|
+
|
162
|
+
dead_zone = axis_conf[:dead_zone] || "0:0" # % deadzone around 0
|
163
|
+
dz_start, dz_end = dead_zone.split(':')
|
164
|
+
dead_zone_range = Range.new(dz_start.to_i, dz_end.to_i)
|
165
|
+
|
166
|
+
value = @joystick.axis(axis_id)
|
167
|
+
value *= -1 if invert
|
168
|
+
value += calibration
|
169
|
+
|
170
|
+
# Make sure input falls with the expected range
|
171
|
+
if value > input_range.last then value = input_range.last end
|
172
|
+
if value < input_range.first then value = input_range.first end
|
173
|
+
# Dead zone
|
174
|
+
|
175
|
+
if dead_zone_range.first < value && dead_zone_range.last > value
|
176
|
+
value = 0
|
177
|
+
end
|
178
|
+
# Convert
|
179
|
+
if is_thrust
|
180
|
+
value = pre_normalize_thrust(value, input_range, output_range)
|
181
|
+
value = normalize_thrust(value)
|
182
|
+
else
|
183
|
+
value = normalize(value, input_range, output_range)
|
184
|
+
end
|
185
|
+
|
186
|
+
# Check if we change too fast
|
187
|
+
current_time = Time.now.to_f
|
188
|
+
timespan = current_time - last_poll
|
189
|
+
# How many ms have passed since last time
|
190
|
+
timespan_ms = timespan.round(3) * 1000
|
191
|
+
# How much have we changed/ms
|
192
|
+
change = (value - last_value) / timespan_ms.to_f
|
193
|
+
|
194
|
+
# Skip rate limitation if change is positive and this is thurst
|
195
|
+
if !is_thrust || (is_thrust && change <= 0)
|
196
|
+
# If the change rate exceeds the max change rate per ms...
|
197
|
+
if change.abs > max_chrate
|
198
|
+
# new value is the max change possible for the timespan
|
199
|
+
if change > 0
|
200
|
+
value = last_value + max_chrate * timespan_ms
|
201
|
+
elsif change < 0
|
202
|
+
value = last_value - max_chrate * timespan_ms
|
203
|
+
end
|
204
|
+
end
|
205
|
+
end
|
206
|
+
|
207
|
+
@config[:axis][axis_id][:last_poll] = current_time
|
208
|
+
@config[:axis][axis_id][:last_value] = value
|
209
|
+
|
210
|
+
return value
|
211
|
+
end
|
212
|
+
|
213
|
+
# The thrust axis is disabled for values < 0. What we do here is to
|
214
|
+
# convert it to a thrust 0-100% value first and then make sure it
|
215
|
+
# stays within the limits provided for output range
|
216
|
+
def pre_normalize_thrust(value, input_range, output_range)
|
217
|
+
value = normalize(value, input_range, (-100..100))
|
218
|
+
value = 0 if value < 0
|
219
|
+
if value > output_range.last then value = output_range.last
|
220
|
+
elsif value < output_range.first then value = output_range.first
|
221
|
+
end
|
222
|
+
return value
|
223
|
+
end
|
224
|
+
private :pre_normalize_thrust
|
225
|
+
|
226
|
+
|
227
|
+
# Returns integer from 10.000 to 60.000 which is what the crazyflie
|
228
|
+
# expects
|
229
|
+
def normalize_thrust(value)
|
230
|
+
return normalize(value, (0..100), (9500..60000)).round
|
231
|
+
end
|
232
|
+
private :normalize_thrust
|
233
|
+
|
234
|
+
# Reads the specified joystick button. Since we may want to calibrate
|
235
|
+
# buttons etc, we return an integer. In only reads the button if
|
236
|
+
# the reading has not be done in the last 1 second to avoid
|
237
|
+
# flapping
|
238
|
+
# @param button_id [Integer] the SDL button number
|
239
|
+
# @return [Fixnum] -1 if the button is not pressed, 1 otherwise
|
240
|
+
def read_button(button_id)
|
241
|
+
return -1 if !@joystick
|
242
|
+
|
243
|
+
last_poll = @config[:buttons][button_id][:last_poll] || 0
|
244
|
+
last_value = @config[:buttons][button_id][:last_value] || -1
|
245
|
+
value = @joystick.button(button_id) ? 1 : -1
|
246
|
+
current_time = Time.now.to_f
|
247
|
+
|
248
|
+
if (current_time - last_poll) > 0.5
|
249
|
+
@config[:buttons][button_id][:last_value] = value
|
250
|
+
@config[:buttons][button_id][:last_poll] = current_time
|
251
|
+
return value
|
252
|
+
else
|
253
|
+
return -1
|
254
|
+
end
|
255
|
+
end
|
256
|
+
private :read_button
|
257
|
+
|
258
|
+
# Called before reading
|
259
|
+
def poll
|
260
|
+
SDL::Joystick.update_all
|
261
|
+
end
|
262
|
+
private :poll
|
263
|
+
|
264
|
+
# Linear-transforms a value in one range to a different range
|
265
|
+
# @param value [Fixnum, Float] the value in the original range
|
266
|
+
# @param from_range [Range] the range from which we want to normalize
|
267
|
+
# @param to_range [Range] the destination range
|
268
|
+
# @return [Float] the linear-corresponding value in the destination
|
269
|
+
# range
|
270
|
+
def normalize(value, from_range, to_range)
|
271
|
+
from_w = from_range.size.to_f - 1
|
272
|
+
to_w = to_range.size.to_f - 1
|
273
|
+
from_min = from_range.first.to_f
|
274
|
+
to_min = to_range.first.to_f
|
275
|
+
# puts "#{to_min}+(#{value.to_f}-#{from_min})*(#{to_w}/#{from_w})
|
276
|
+
r = to_min + (value.to_f - from_min) * (to_w / from_w)
|
277
|
+
return r.round(3)
|
278
|
+
end
|
279
|
+
end
|
280
|
+
end
|
@@ -0,0 +1,22 @@
|
|
1
|
+
# -*- coding: utf-8 -*-
|
2
|
+
# Copyright (C) 2013 Hector Sanjuan
|
3
|
+
|
4
|
+
# This file is part of Crubyflie.
|
5
|
+
|
6
|
+
# Crubyflie is free software: you can redistribute it and/or modify
|
7
|
+
# it under the terms of the GNU General Public License as published by
|
8
|
+
# the Free Software Foundation, either version 3 of the License, or
|
9
|
+
# (at your option) any later version.
|
10
|
+
|
11
|
+
# Crubyflie is distributed in the hope that it will be useful,
|
12
|
+
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
13
|
+
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
14
|
+
# GNU General Public License for more details.
|
15
|
+
|
16
|
+
# You should have received a copy of the GNU General Public License
|
17
|
+
# along with Crubyflie. If not, see <http://www.gnu.org/licenses/>
|
18
|
+
|
19
|
+
module Crubyflie
|
20
|
+
# Current gem version
|
21
|
+
VERSION = "0.1.0"
|
22
|
+
end
|
data/lib/crubyflie.rb
ADDED
@@ -0,0 +1,31 @@
|
|
1
|
+
# -*- coding: utf-8 -*-
|
2
|
+
# Copyright (C) 2013 Hector Sanjuan
|
3
|
+
|
4
|
+
# This file is part of Crubyflie.
|
5
|
+
|
6
|
+
# Crubyflie is free software: you can redistribute it and/or modify
|
7
|
+
# it under the terms of the GNU General Public License as published by
|
8
|
+
# the Free Software Foundation, either version 3 of the License, or
|
9
|
+
# (at your option) any later version.
|
10
|
+
|
11
|
+
# Crubyflie is distributed in the hope that it will be useful,
|
12
|
+
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
13
|
+
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
14
|
+
# GNU General Public License for more details.
|
15
|
+
|
16
|
+
# You should have received a copy of the GNU General Public License
|
17
|
+
# along with Crubyflie. If not, see <http://www.gnu.org/licenses/>
|
18
|
+
|
19
|
+
$: << File.join(File.dirname(__FILE__), 'crubyflie')
|
20
|
+
|
21
|
+
require 'crubyflie_logger'
|
22
|
+
require 'crazyflie'
|
23
|
+
require 'input/joystick_input_reader'
|
24
|
+
require 'version'
|
25
|
+
|
26
|
+
# The Crubyflie modules wraps all the Crubyflie code so we don't
|
27
|
+
# pollute the namespace.
|
28
|
+
module Crubyflie
|
29
|
+
$debug = false
|
30
|
+
include Logging
|
31
|
+
end
|
@@ -0,0 +1,67 @@
|
|
1
|
+
# -*- coding: utf-8 -*-
|
2
|
+
# Copyright (C) 2013 Hector Sanjuan
|
3
|
+
|
4
|
+
# This file is part of Crubyflie.
|
5
|
+
|
6
|
+
# Crubyflie is free software: you can redistribute it and/or modify
|
7
|
+
# it under the terms of the GNU General Public License as published by
|
8
|
+
# the Free Software Foundation, either version 3 of the License, or
|
9
|
+
# (at your option) any later version.
|
10
|
+
|
11
|
+
# Crubyflie is distributed in the hope that it will be useful,
|
12
|
+
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
13
|
+
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
14
|
+
# GNU General Public License for more details.
|
15
|
+
|
16
|
+
# You should have received a copy of the GNU General Public License
|
17
|
+
# along with Crubyflie. If not, see <http://www.gnu.org/licenses/>
|
18
|
+
|
19
|
+
require 'crazyflie/commander'
|
20
|
+
|
21
|
+
describe Commander do
|
22
|
+
|
23
|
+
before :each do
|
24
|
+
@crazyflie = double("Crazyflie")
|
25
|
+
@queue = Queue.new
|
26
|
+
allow(@crazyflie).to receive(:crtp_queues).and_return({:commander =>
|
27
|
+
@queue})
|
28
|
+
|
29
|
+
@commander = Commander.new(@crazyflie)
|
30
|
+
end
|
31
|
+
|
32
|
+
describe "#initialize" do
|
33
|
+
it "should initialize the facility" do
|
34
|
+
c = Commander.new(@crazyflie)
|
35
|
+
end
|
36
|
+
end
|
37
|
+
|
38
|
+
describe "#send_sendpoint" do
|
39
|
+
it "should send a sendpoint in normal mode" do
|
40
|
+
expect(@crazyflie).to receive(:send_packet) do |packet, want_answer|
|
41
|
+
want_answer.should == false
|
42
|
+
packet.port.should == Crazyflie::CRTP_PORTS[:commander]
|
43
|
+
packet.channel.should == 0
|
44
|
+
packet.data.size.should == 14
|
45
|
+
packet.data_repack.unpack('eeeS<').should == [1,-2,3,6]
|
46
|
+
end
|
47
|
+
|
48
|
+
@commander.send_setpoint(1,2,3,6)
|
49
|
+
end
|
50
|
+
|
51
|
+
it "should send a sendpoint in xmode" do
|
52
|
+
expect(@crazyflie).to receive(:send_packet) do |packet, want_answer|
|
53
|
+
want_answer.should == false
|
54
|
+
packet.port.should == Crazyflie::CRTP_PORTS[:commander]
|
55
|
+
packet.channel.should == 0
|
56
|
+
packet.data.size.should == 14
|
57
|
+
data = packet.data_repack.unpack('eeeS<')
|
58
|
+
data[0].round(3).should == -0.707
|
59
|
+
data[1].round(4).should == -0.9142
|
60
|
+
data[2].should == 3.0
|
61
|
+
data[3].should == 6
|
62
|
+
end
|
63
|
+
|
64
|
+
@commander.send_setpoint(1,2,3,6, true)
|
65
|
+
end
|
66
|
+
end
|
67
|
+
end
|
@@ -0,0 +1,76 @@
|
|
1
|
+
# -*- coding: utf-8 -*-
|
2
|
+
# Copyright (C) 2013 Hector Sanjuan
|
3
|
+
|
4
|
+
# This file is part of Crubyflie.
|
5
|
+
|
6
|
+
# Crubyflie is free software: you can redistribute it and/or modify
|
7
|
+
# it under the terms of the GNU General Public License as published by
|
8
|
+
# the Free Software Foundation, either version 3 of the License, or
|
9
|
+
# (at your option) any later version.
|
10
|
+
|
11
|
+
# Crubyflie is distributed in the hope that it will be useful,
|
12
|
+
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
13
|
+
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
14
|
+
# GNU General Public License for more details.
|
15
|
+
|
16
|
+
# You should have received a copy of the GNU General Public License
|
17
|
+
# along with Crubyflie. If not, see <http://www.gnu.org/licenses/>
|
18
|
+
|
19
|
+
require 'crazyflie/console'
|
20
|
+
|
21
|
+
describe Console do
|
22
|
+
before :each do
|
23
|
+
@crazyflie = double("Crazyflie")
|
24
|
+
@queue = Queue.new
|
25
|
+
allow(@crazyflie).to receive(:crtp_queues).and_return({:console =>
|
26
|
+
@queue})
|
27
|
+
|
28
|
+
@console = Console.new(@crazyflie)
|
29
|
+
end
|
30
|
+
describe "#initialize" do
|
31
|
+
it "should intialize the facility" do
|
32
|
+
c = Console.new(@crazyflie)
|
33
|
+
expect_any_instance_of(Thread).not_to receive(:new)
|
34
|
+
end
|
35
|
+
end
|
36
|
+
|
37
|
+
describe "#read" do
|
38
|
+
it "should read all packets available from the queue" do
|
39
|
+
count = 1
|
40
|
+
p1 = CRTPPacket.new()
|
41
|
+
p1.data = "baa1".unpack('C*')
|
42
|
+
p2 = CRTPPacket.new()
|
43
|
+
p2.data = "baa2".unpack('C*')
|
44
|
+
|
45
|
+
@queue << p1
|
46
|
+
@queue << p2
|
47
|
+
|
48
|
+
@console.read do |message|
|
49
|
+
message.should == "baa#{count}"
|
50
|
+
count += 1
|
51
|
+
end
|
52
|
+
@queue.size.should == 0
|
53
|
+
end
|
54
|
+
end
|
55
|
+
|
56
|
+
describe "#start_reading" do
|
57
|
+
it "should read continuously" do
|
58
|
+
count = 1
|
59
|
+
p1 = CRTPPacket.new()
|
60
|
+
p1.data = "baa1".unpack('C*')
|
61
|
+
p2 = CRTPPacket.new()
|
62
|
+
p2.data = "baa2".unpack('C*')
|
63
|
+
|
64
|
+
@queue << p1
|
65
|
+
@queue << p2
|
66
|
+
|
67
|
+
@console.start_reading do |message|
|
68
|
+
message.should == "baa#{count}"
|
69
|
+
count += 1
|
70
|
+
end
|
71
|
+
sleep 0.3
|
72
|
+
@queue.size.should == 0
|
73
|
+
@console.stop_reading()
|
74
|
+
end
|
75
|
+
end
|
76
|
+
end
|
@@ -0,0 +1,176 @@
|
|
1
|
+
# -*- coding: utf-8 -*-
|
2
|
+
# Copyright (C) 2013 Hector Sanjuan
|
3
|
+
|
4
|
+
# This file is part of Crubyflie.
|
5
|
+
|
6
|
+
# Crubyflie is free software: you can redistribute it and/or modify
|
7
|
+
# it under the terms of the GNU General Public License as published by
|
8
|
+
# the Free Software Foundation, either version 3 of the License, or
|
9
|
+
# (at your option) any later version.
|
10
|
+
|
11
|
+
# Crubyflie is distributed in the hope that it will be useful,
|
12
|
+
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
13
|
+
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
14
|
+
# GNU General Public License for more details.
|
15
|
+
|
16
|
+
# You should have received a copy of the GNU General Public License
|
17
|
+
# along with Crubyflie. If not, see <http://www.gnu.org/licenses/>
|
18
|
+
|
19
|
+
require 'crazyflie'
|
20
|
+
|
21
|
+
|
22
|
+
describe Crazyflie do
|
23
|
+
before :all do
|
24
|
+
end
|
25
|
+
before :each do
|
26
|
+
@facility = double("Facility")
|
27
|
+
allow(Commander).to receive(:new).and_return(@facility)
|
28
|
+
allow(Param).to receive(:new).and_return(@facility)
|
29
|
+
allow(Log).to receive(:new).and_return(@facility)
|
30
|
+
allow(Commander).to receive(:new).and_return(@facility)
|
31
|
+
allow(@facility).to receive(:send_setpoint)
|
32
|
+
allow(@facility).to receive(:refresh_toc)
|
33
|
+
allow(@facility).to receive(:start_packet_reader_thread)
|
34
|
+
allow(@facility).to receive(:stop_packet_reader_thread)
|
35
|
+
@link = double("RadioDriver")
|
36
|
+
@uri = 'radio://0/0/1M'
|
37
|
+
allow(RadioDriver).to receive(:new).and_return(@link)
|
38
|
+
allow(@link).to receive(:connect)
|
39
|
+
allow(@link).to receive(:disconnect)
|
40
|
+
allow(@link).to receive(:receive_packet)
|
41
|
+
allow(@link).to receive(:uri).and_return(URI(@uri))
|
42
|
+
|
43
|
+
#allow_any_instance_of(CrubyflieLogger).to receive(:info)
|
44
|
+
|
45
|
+
@cf = Crazyflie.new()
|
46
|
+
@logger = @cf.logger
|
47
|
+
|
48
|
+
@default_pk = CRTPPacket.new(0b00001100, [1,2,3])
|
49
|
+
end
|
50
|
+
|
51
|
+
|
52
|
+
describe "#initialize" do
|
53
|
+
it "should initalize correctly" do
|
54
|
+
Crazyflie::CALLBACKS.each do |cb|
|
55
|
+
@cf.callbacks[cb].size.should >= 0
|
56
|
+
end
|
57
|
+
|
58
|
+
Crazyflie::CRTP_PORTS.keys().each do |port|
|
59
|
+
@cf.crtp_queues[port].should be_an_instance_of Queue
|
60
|
+
end
|
61
|
+
end
|
62
|
+
end
|
63
|
+
|
64
|
+
|
65
|
+
describe "#open_link" do
|
66
|
+
it "should connect to a crazyradio url" do
|
67
|
+
expect(@link).to receive(:receive_packet).and_return(@default_pk)
|
68
|
+
m = "Connection initiated to radio://0/0/1M"
|
69
|
+
m2 = "Disconnected from radio://0/0/1M"
|
70
|
+
m3 = "TOCs extracted from #{@uri}"
|
71
|
+
expect(@logger).to receive(:info).with(m)
|
72
|
+
expect(@logger).to receive(:info).with("Connected!")
|
73
|
+
expect(@logger).to receive(:info).with("Connection ready!")
|
74
|
+
expect(@logger).to receive(:info).with(m2)
|
75
|
+
expect(@logger).to receive(:debug).with(m3)
|
76
|
+
|
77
|
+
|
78
|
+
expect(@facility).to receive(:refresh_toc).twice
|
79
|
+
# only log facility gets this
|
80
|
+
expect(@facility).to receive(:start_packet_reader_thread).once
|
81
|
+
expect(@facility).to receive(:stop_packet_reader_thread).twice
|
82
|
+
@cf.open_link(@uri)
|
83
|
+
@cf.close_link()
|
84
|
+
end
|
85
|
+
|
86
|
+
it "should close the link if something happens" do
|
87
|
+
expect(@link).to receive(:connect).and_raise(Exception)
|
88
|
+
mesg = "Connection failed: Exception"
|
89
|
+
expect(@logger).to receive(:info).at_least(:once)
|
90
|
+
expect(@logger).to receive(:error).with(mesg)
|
91
|
+
expect(@facility).not_to receive(:refresh_toc)
|
92
|
+
@cf.open_link(@uri)
|
93
|
+
@cf.close_link()
|
94
|
+
end
|
95
|
+
end
|
96
|
+
|
97
|
+
describe "#close_link" do
|
98
|
+
it "should close the link" do
|
99
|
+
expect(@facility).to receive(:send_setpoint)
|
100
|
+
expect(@link).to receive(:disconnect)
|
101
|
+
m1 = "Connection initiated to radio://0/0/1M"
|
102
|
+
m2 = "Connection ready!"
|
103
|
+
m3 = "Disconnected from radio://0/0/1M"
|
104
|
+
expect(@logger).to receive(:debug)
|
105
|
+
expect(@logger).to receive(:info).with(m1)
|
106
|
+
expect(@logger).to receive(:info).with(m2)
|
107
|
+
expect(@logger).to receive(:info).with(m3)
|
108
|
+
|
109
|
+
@cf.open_link(@uri)
|
110
|
+
@cf.close_link()
|
111
|
+
@cf.crtp_queues.each do |k,q|
|
112
|
+
q.should be_empty
|
113
|
+
end
|
114
|
+
end
|
115
|
+
|
116
|
+
it "should not break closing a non existing link" do
|
117
|
+
expect(@facility).not_to receive(:send_setpoint)
|
118
|
+
expect_any_instance_of(NilClass).not_to receive(:disconnect)
|
119
|
+
expect(@cf).not_to receive(:puts)
|
120
|
+
expect_any_instance_of(Thread).not_to receive(:kill)
|
121
|
+
expect(@logger).to receive(:info).with("Disconnected from nowhere")
|
122
|
+
@cf.close_link()
|
123
|
+
end
|
124
|
+
end
|
125
|
+
|
126
|
+
describe "#send_packet" do
|
127
|
+
it "should send a packet without expecting answer" do
|
128
|
+
expect(@cf).not_to receive(:setup_retry)
|
129
|
+
expect(@link).to receive(:send_packet).with(@default_pk)
|
130
|
+
expect(@logger).to receive(:info).at_least(:once)
|
131
|
+
expect(@logger).to receive(:debug)
|
132
|
+
@cf.open_link(@uri)
|
133
|
+
@cf.send_packet(@default_pk)
|
134
|
+
@cf.close_link()
|
135
|
+
end
|
136
|
+
|
137
|
+
it "should send a packet and set up a timer when expecting answer" do
|
138
|
+
pk = @default_pk
|
139
|
+
expect(@link).to receive(:send_packet).with(pk).at_least(:twice)
|
140
|
+
expect(@logger).to receive(:info).at_least(:once)
|
141
|
+
expect(@logger).to receive(:debug).at_least(:once)
|
142
|
+
@cf.open_link(@uri)
|
143
|
+
@cf.send_packet(@default_pk, true)
|
144
|
+
sleep 0.5
|
145
|
+
@cf.close_link()
|
146
|
+
end
|
147
|
+
end
|
148
|
+
|
149
|
+
describe "#receive_packet" do
|
150
|
+
it " should receive a packet, trigger callbacks" do
|
151
|
+
proc = Proc.new do
|
152
|
+
puts "port 0 ch 0 callback"
|
153
|
+
end
|
154
|
+
|
155
|
+
proc2 = Proc.new do |pk|
|
156
|
+
puts "Received packet"
|
157
|
+
end
|
158
|
+
|
159
|
+
@cf.callbacks[:received_packet][:log] = proc
|
160
|
+
@cf.callbacks[:received_packet][:log2] = proc2
|
161
|
+
|
162
|
+
allow_any_instance_of(Thread).to receive(:new).and_return(nil)
|
163
|
+
expect(@link).to receive(:receive_packet).and_return(@default_pk)
|
164
|
+
expect(proc).to receive(:call).with(@default_pk).at_least(:once)
|
165
|
+
expect(proc2).to receive(:call).with(@default_pk).at_least(:once)
|
166
|
+
expect(@cf.crtp_queues[:console]).to receive(:<<).once
|
167
|
+
expect(@logger).to receive(:info).at_least(:once)
|
168
|
+
expect(@logger).to receive(:debug)
|
169
|
+
@cf.open_link(@uri)
|
170
|
+
@cf.send(:receive_packet)
|
171
|
+
# Received packet comes on port 0 - console
|
172
|
+
|
173
|
+
@cf.close_link()
|
174
|
+
end
|
175
|
+
end
|
176
|
+
end
|