px4_log_reader 0.0.5
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 +4 -0
- data/README.md +43 -0
- data/Rakefile +124 -0
- data/lib/px4_log_reader/invalid_descriptor_error.rb +25 -0
- data/lib/px4_log_reader/log_buffer.rb +153 -0
- data/lib/px4_log_reader/log_file.rb +125 -0
- data/lib/px4_log_reader/log_message.rb +30 -0
- data/lib/px4_log_reader/message_descriptor.rb +185 -0
- data/lib/px4_log_reader/message_descriptor_cache.rb +56 -0
- data/lib/px4_log_reader/reader.rb +160 -0
- data/lib/px4_log_reader/version.rb +3 -0
- data/lib/px4_log_reader.rb +8 -0
- data/lib/px4log_parser.rb +228 -0
- data/px4_log_reader.gemspec +17 -0
- data/test/test.rb +33 -0
- data/test/test_files/pixhawk_descriptor_cache.px4mc +0 -0
- data/test/test_files/test_log.px4log +0 -0
- data/test/test_log_buffer.rb +173 -0
- data/test/test_log_file.rb +44 -0
- data/test/test_message_descriptor.rb +56 -0
- data/test/test_message_descriptor_cache.rb +70 -0
- data/test/test_reader.rb +127 -0
- metadata +81 -0
checksums.yaml
ADDED
@@ -0,0 +1,7 @@
|
|
1
|
+
---
|
2
|
+
SHA1:
|
3
|
+
metadata.gz: 4866bef24e75e23430bf53f0d79860ce51179031
|
4
|
+
data.tar.gz: 99c4506ee9f1324a75be0967a1e4d0e16b92410b
|
5
|
+
SHA512:
|
6
|
+
metadata.gz: 4fbfd8dfa77d550d8bc7139fdae14600d50d5450d017b881b2a45c478f35fff60fa835b77cbd3697dcf5b2d6d19ce66182e0ee2204e6eb4ce21c77752aa35487
|
7
|
+
data.tar.gz: e5d5c1eb3fa33137568d1908c40cd68249066809c61ca0a1b360f59fe69f9ec7f1b49dadf08f802084d8ceacb6f34b83ff4eed44e188ba01aae096777df7fd8e
|
data/.gitignore
ADDED
data/README.md
ADDED
@@ -0,0 +1,43 @@
|
|
1
|
+
[](https://travis-ci.org/rgmann/px4_log_reader)
|
2
|
+
|
3
|
+
## Welcome to Px4LogReader ##
|
4
|
+
|
5
|
+
Px4LogReader is a Ruby gem for parsing PX4 © self-describing log files.
|
6
|
+
|
7
|
+
|
8
|
+
## Install the gem ##
|
9
|
+
|
10
|
+
Install it with [RubyGems](https://rubygems.org/)
|
11
|
+
|
12
|
+
gem install px4_log_reader
|
13
|
+
|
14
|
+
or add this to your Gemfile if you use [Bundler](http://gembundler.com/):
|
15
|
+
|
16
|
+
gem "px4_log_reader"
|
17
|
+
|
18
|
+
|
19
|
+
## Getting Started ##
|
20
|
+
|
21
|
+
### Example: Read PX4 log file ###
|
22
|
+
|
23
|
+
require 'px4_log_reader'
|
24
|
+
|
25
|
+
Px4LogReader.open( 'a_test_log.px4log' ) do |reader|
|
26
|
+
reader.each_message( { with: [ 'ATT' ] } ) do |message,context|
|
27
|
+
|
28
|
+
att = [ messaged.get('roll'), message.get('pitch'), message.get('yaw') ]
|
29
|
+
|
30
|
+
puts "ATT( @ #{context.find_by_name('GPS').get('time')} ): roll=#{att[0]}, pitch=#{att[1]}, yaw=#{att[2]}"
|
31
|
+
|
32
|
+
end
|
33
|
+
end
|
34
|
+
|
35
|
+
|
36
|
+
## License and copyright ##
|
37
|
+
|
38
|
+
Px4LogReader is released under the BSD License.
|
39
|
+
|
40
|
+
Copyright: © 2016 by Robert Glissmann. All Rights Reserved.
|
41
|
+
|
42
|
+
"PX4" is a copyright of PX4 Autopilot (aka PX4 Dev Team). All Rights Reserved.
|
43
|
+
|
data/Rakefile
ADDED
@@ -0,0 +1,124 @@
|
|
1
|
+
require 'rake/testtask'
|
2
|
+
|
3
|
+
Rake::TestTask.new do |t|
|
4
|
+
t.libs << 'test'
|
5
|
+
end
|
6
|
+
|
7
|
+
desc "Run tests"
|
8
|
+
task :default => :test
|
9
|
+
|
10
|
+
def read_default_log_filename
|
11
|
+
log_filename = ''
|
12
|
+
File.open( 'default_log_path.txt', 'r' ) do |input|
|
13
|
+
log_filename = input.readline
|
14
|
+
end
|
15
|
+
|
16
|
+
unless File.exist? log_filename
|
17
|
+
raise "Failed to find '#{log_filename}'"
|
18
|
+
end
|
19
|
+
|
20
|
+
return log_filename
|
21
|
+
end
|
22
|
+
|
23
|
+
task :build_minor, :version do |t,args|
|
24
|
+
|
25
|
+
Dir.glob('px4_log_reader-*.gem') do |gem_file|
|
26
|
+
puts "Deleting '#{gem_file}'"
|
27
|
+
FileUtils.rm gem_file
|
28
|
+
end
|
29
|
+
|
30
|
+
version_file = File.join( 'lib', 'px4_log_reader', 'version.rb' )
|
31
|
+
current_version = '0.0.1'
|
32
|
+
|
33
|
+
if File.exist?( File.expand_path( version_file ) )
|
34
|
+
require_relative version_file
|
35
|
+
current_version = Px4LogReader::VERSION
|
36
|
+
end
|
37
|
+
|
38
|
+
args.with_defaults( :version => current_version.next )
|
39
|
+
next_version = args[:version]
|
40
|
+
|
41
|
+
File.open( version_file, 'w+' ) do |output|
|
42
|
+
puts "Generation '#{version_file}' for version '#{next_version}'"
|
43
|
+
output.write "module Px4LogReader\n\tVERSION = '#{next_version}'\nend"
|
44
|
+
end
|
45
|
+
|
46
|
+
`gem uninstall px4_log_reader && gem build px4_log_reader.gemspec && gem install ./px4_log_reader-#{next_version}.gem`
|
47
|
+
end
|
48
|
+
|
49
|
+
task :gen_desc_cache do
|
50
|
+
require 'px4_log_reader'
|
51
|
+
|
52
|
+
log_filename = read_default_log_filename
|
53
|
+
|
54
|
+
log_file = File.open( log_filename, 'r' )
|
55
|
+
descriptors = Px4LogReader::LogFile.read_descriptors( log_file )
|
56
|
+
|
57
|
+
cache_filename = File.join( 'test', 'test_files', 'pixhawk_descriptor_cache.px4mc' )
|
58
|
+
cache = Px4LogReader::MessageDescriptorCache.new( cache_filename )
|
59
|
+
cache.write_descriptors( descriptors )
|
60
|
+
|
61
|
+
end
|
62
|
+
|
63
|
+
task :dump_desc_cache do
|
64
|
+
require 'px4_log_reader'
|
65
|
+
|
66
|
+
cache_filename = File.join( 'test', 'test_files', 'pixhawk_descriptor_cache.px4mc' )
|
67
|
+
cache = Px4LogReader::MessageDescriptorCache.new( cache_filename )
|
68
|
+
descriptors = cache.read_descriptors
|
69
|
+
|
70
|
+
descriptors.each do |type,descriptor|
|
71
|
+
puts [descriptor.name,'%02X'%descriptor.type].concat(descriptor.field_list.keys).join(',')
|
72
|
+
end
|
73
|
+
end
|
74
|
+
|
75
|
+
task :cut_log, :count, :skip, :filename do |t,args|
|
76
|
+
args.with_defaults(:count => 20, :skip => 0, :filename => File.join('test','test_files','test_log.px4log'))
|
77
|
+
|
78
|
+
require 'px4_log_reader'
|
79
|
+
|
80
|
+
input_filename = read_default_log_filename
|
81
|
+
|
82
|
+
cache_filename = File.join( 'test', 'test_files', 'pixhawk_descriptor_cache.px4mc' )
|
83
|
+
cache = Px4LogReader::MessageDescriptorCache.new( cache_filename )
|
84
|
+
message_descriptors = cache.read_descriptors
|
85
|
+
|
86
|
+
|
87
|
+
File.open( input_filename, 'r' ) do |input|
|
88
|
+
File.open( args[:filename], 'wb+' ) do |output|
|
89
|
+
|
90
|
+
# Write the descriptors
|
91
|
+
message_descriptors.each do |type,descriptor|
|
92
|
+
Px4LogReader::LogFile.write_message( output, descriptor.to_message )
|
93
|
+
end
|
94
|
+
|
95
|
+
skip_count = args[:skip].to_i
|
96
|
+
skip_count.times do
|
97
|
+
Px4LogReader::LogFile.read_message( input, message_descriptors )
|
98
|
+
end
|
99
|
+
|
100
|
+
read_count = args[:count].to_i
|
101
|
+
read_count.times do
|
102
|
+
message = Px4LogReader::LogFile.read_message( input, message_descriptors )
|
103
|
+
Px4LogReader::LogFile.write_message( output, message )
|
104
|
+
end
|
105
|
+
end
|
106
|
+
end
|
107
|
+
|
108
|
+
end
|
109
|
+
|
110
|
+
task :dump, :filename do |t,args|
|
111
|
+
require 'px4_log_reader'
|
112
|
+
|
113
|
+
cache_filename = File.join( 'test', 'test_files', 'pixhawk_descriptor_cache.px4mc' )
|
114
|
+
cache = Px4LogReader::MessageDescriptorCache.new( cache_filename )
|
115
|
+
message_descriptors = cache.read_descriptors
|
116
|
+
|
117
|
+
File.open( args[:filename], 'r' ) do |input|
|
118
|
+
count = 1
|
119
|
+
while ( message = Px4LogReader::LogFile.read_message( input, message_descriptors ) ) do
|
120
|
+
puts "#{count}) #{message.descriptor.name}, #{'%02X'%message.descriptor.type}"
|
121
|
+
count += 1
|
122
|
+
end
|
123
|
+
end
|
124
|
+
end
|
@@ -0,0 +1,25 @@
|
|
1
|
+
module Px4LogReader
|
2
|
+
|
3
|
+
class Error < StandardError
|
4
|
+
|
5
|
+
def initialize( message='' )
|
6
|
+
super( message )
|
7
|
+
end
|
8
|
+
|
9
|
+
end
|
10
|
+
|
11
|
+
class InvalidDescriptorError < Error
|
12
|
+
|
13
|
+
def initialize( message )
|
14
|
+
super( message )
|
15
|
+
end
|
16
|
+
|
17
|
+
end
|
18
|
+
|
19
|
+
class FileNotFoundError < Error
|
20
|
+
def initialize( filename )
|
21
|
+
super( "Failed to find '#{filename}'" )
|
22
|
+
end
|
23
|
+
end
|
24
|
+
|
25
|
+
end
|
@@ -0,0 +1,153 @@
|
|
1
|
+
module Px4LogReader
|
2
|
+
|
3
|
+
class LogBuffer
|
4
|
+
|
5
|
+
attr_reader :data
|
6
|
+
attr_reader :read_position
|
7
|
+
attr_reader :write_position
|
8
|
+
|
9
|
+
def initialize( size )
|
10
|
+
@data = Array.new( size, 0x00 )
|
11
|
+
@read_position = 0
|
12
|
+
@write_position = 0
|
13
|
+
end
|
14
|
+
|
15
|
+
def reset
|
16
|
+
@read_position = 0
|
17
|
+
@write_position = 0
|
18
|
+
end
|
19
|
+
|
20
|
+
def write( file )
|
21
|
+
while ( @write_position < @data.size ) do
|
22
|
+
begin
|
23
|
+
|
24
|
+
bytes = file.read( @data.size - @write_position )
|
25
|
+
|
26
|
+
if bytes
|
27
|
+
write_bytes( bytes.unpack('C*') )
|
28
|
+
else
|
29
|
+
break
|
30
|
+
end
|
31
|
+
|
32
|
+
rescue EOFError => error
|
33
|
+
break
|
34
|
+
end
|
35
|
+
end
|
36
|
+
end
|
37
|
+
|
38
|
+
def read( num_bytes )
|
39
|
+
data = ''
|
40
|
+
|
41
|
+
if !empty?
|
42
|
+
last_index = @read_position + num_bytes
|
43
|
+
last_index = @data.size if last_index > @data.size
|
44
|
+
|
45
|
+
read_count = last_index - @read_position
|
46
|
+
data = @data[ @read_position, read_count ].pack('C*')
|
47
|
+
@read_position += read_count
|
48
|
+
end
|
49
|
+
|
50
|
+
return data
|
51
|
+
end
|
52
|
+
|
53
|
+
def empty?
|
54
|
+
return ( @read_position == @write_position )
|
55
|
+
end
|
56
|
+
|
57
|
+
protected
|
58
|
+
|
59
|
+
def write_bytes( bytes )
|
60
|
+
bytes.each do |byte|
|
61
|
+
if @write_position < @data.size
|
62
|
+
@data[ @write_position ] = byte
|
63
|
+
@write_position += 1
|
64
|
+
end
|
65
|
+
end
|
66
|
+
end
|
67
|
+
end
|
68
|
+
|
69
|
+
class LogBufferArray
|
70
|
+
|
71
|
+
attr_reader :buffers
|
72
|
+
attr_reader :current_buffer_index
|
73
|
+
|
74
|
+
def initialize
|
75
|
+
@buffers = []
|
76
|
+
@active_file = nil
|
77
|
+
@current_buffer_index = 0
|
78
|
+
end
|
79
|
+
|
80
|
+
def set_file( file, options = {} )
|
81
|
+
|
82
|
+
opts = {
|
83
|
+
buffer_count: 2,
|
84
|
+
load_buffers: true,
|
85
|
+
buffer_size: 1024
|
86
|
+
}.merge( options )
|
87
|
+
|
88
|
+
@active_file = file
|
89
|
+
@buffer_count = opts[:buffer_count]
|
90
|
+
@buffer_size = opts[:buffer_size] / opts[:buffer_count]
|
91
|
+
|
92
|
+
load_buffers if opts[:load_buffers]
|
93
|
+
end
|
94
|
+
|
95
|
+
def load_buffers
|
96
|
+
|
97
|
+
@buffers = []
|
98
|
+
@buffer_count.times do
|
99
|
+
@buffers << LogBuffer.new( @buffer_size )
|
100
|
+
end
|
101
|
+
|
102
|
+
@buffers.each do |buffer|
|
103
|
+
buffer.write( @active_file )
|
104
|
+
end
|
105
|
+
end
|
106
|
+
|
107
|
+
def read( num_bytes )
|
108
|
+
data = ''
|
109
|
+
|
110
|
+
while ( data.size < num_bytes ) do
|
111
|
+
|
112
|
+
data << active_buffer.read( num_bytes )
|
113
|
+
|
114
|
+
if data.length < num_bytes
|
115
|
+
increment_buffer
|
116
|
+
break if active_buffer.empty?
|
117
|
+
end
|
118
|
+
|
119
|
+
end
|
120
|
+
|
121
|
+
return data
|
122
|
+
end
|
123
|
+
|
124
|
+
def load_empty_buffers
|
125
|
+
|
126
|
+
inactive_buffer_index = ( @current_buffer_index + 1 ) % @buffer_count
|
127
|
+
|
128
|
+
while ( inactive_buffer_index != @current_buffer_index ) do
|
129
|
+
buffer = buffers[ inactive_buffer_index ]
|
130
|
+
|
131
|
+
if buffer.empty?
|
132
|
+
buffer.reset
|
133
|
+
buffer.write( @active_file )
|
134
|
+
end
|
135
|
+
|
136
|
+
inactive_buffer_index = ( inactive_buffer_index + 1 ) % @buffer_count
|
137
|
+
end
|
138
|
+
|
139
|
+
end
|
140
|
+
|
141
|
+
protected
|
142
|
+
|
143
|
+
def active_buffer
|
144
|
+
return @buffers[ @current_buffer_index ]
|
145
|
+
end
|
146
|
+
|
147
|
+
def increment_buffer
|
148
|
+
@current_buffer_index = (@current_buffer_index + 1) % @buffer_count
|
149
|
+
end
|
150
|
+
|
151
|
+
end
|
152
|
+
|
153
|
+
end
|
@@ -0,0 +1,125 @@
|
|
1
|
+
|
2
|
+
module Px4LogReader
|
3
|
+
|
4
|
+
module LogFile
|
5
|
+
|
6
|
+
HEADER_MARKER = [0xA3,0x95]
|
7
|
+
HEADER_LENGTH = 3
|
8
|
+
FORMAT_DESCRIPTOR_TABLE = { FORMAT_MESSAGE.type => FORMAT_MESSAGE }.freeze
|
9
|
+
|
10
|
+
def self.read_descriptors( buffered_io, descriptor_cache=nil )
|
11
|
+
|
12
|
+
message_descriptors = {}
|
13
|
+
|
14
|
+
while ( message_descriptor = read_descriptor( buffered_io ) ) do
|
15
|
+
if !message_descriptors.keys.include? message_descriptor.type
|
16
|
+
message_descriptors[ message_descriptor.type ] = message_descriptor
|
17
|
+
end
|
18
|
+
end
|
19
|
+
|
20
|
+
# If a cache filename was supplied, dump the descriptors to the cache
|
21
|
+
if descriptor_cache
|
22
|
+
descriptor_cache.write_descriptors( message_descriptors )
|
23
|
+
end
|
24
|
+
|
25
|
+
return message_descriptors
|
26
|
+
end
|
27
|
+
|
28
|
+
def self.read_descriptor( buffered_io, skip_corrupt=true )
|
29
|
+
|
30
|
+
message_descriptor = nil
|
31
|
+
|
32
|
+
begin
|
33
|
+
|
34
|
+
descriptor_message = read_message( buffered_io, FORMAT_DESCRIPTOR_TABLE )
|
35
|
+
|
36
|
+
if descriptor_message
|
37
|
+
|
38
|
+
message_descriptor = Px4LogReader::MessageDescriptor.new
|
39
|
+
message_descriptor.from_message( descriptor_message )
|
40
|
+
|
41
|
+
end
|
42
|
+
|
43
|
+
rescue Px4LogReader::InvalidDescriptorError => error
|
44
|
+
|
45
|
+
if skip_corrupt
|
46
|
+
retry
|
47
|
+
else
|
48
|
+
raise error
|
49
|
+
end
|
50
|
+
|
51
|
+
rescue StandardError => e
|
52
|
+
puts "#{e.class}: #{e.message}"
|
53
|
+
puts e.backtrace.join("\n")
|
54
|
+
end
|
55
|
+
|
56
|
+
return message_descriptor
|
57
|
+
end
|
58
|
+
|
59
|
+
def self.read_message( buffered_io, message_descriptors )
|
60
|
+
|
61
|
+
message = nil
|
62
|
+
while message.nil? do
|
63
|
+
message_type = read_message_header( buffered_io )
|
64
|
+
|
65
|
+
if message_type
|
66
|
+
|
67
|
+
message_descriptor = message_descriptors[ message_type ]
|
68
|
+
|
69
|
+
if message_descriptor
|
70
|
+
message_data = buffered_io.read( message_descriptor.length - HEADER_LENGTH )
|
71
|
+
message = message_descriptor.unpack_message( message_data )
|
72
|
+
end
|
73
|
+
|
74
|
+
elsif message_type.nil?
|
75
|
+
break
|
76
|
+
end
|
77
|
+
end
|
78
|
+
|
79
|
+
return message
|
80
|
+
end
|
81
|
+
|
82
|
+
def self.read_message_header( buffered_io )
|
83
|
+
message_type = nil
|
84
|
+
|
85
|
+
begin
|
86
|
+
|
87
|
+
data = buffered_io.read(2)
|
88
|
+
|
89
|
+
if data && data.length == 2
|
90
|
+
|
91
|
+
while !data.empty? && message_type.nil? do
|
92
|
+
|
93
|
+
if ( byte = buffered_io.read(1) )
|
94
|
+
data << byte
|
95
|
+
end
|
96
|
+
|
97
|
+
if data.unpack('CCC')[0,2] == HEADER_MARKER
|
98
|
+
message_type = data.unpack('CCC').last & 0xFF
|
99
|
+
else
|
100
|
+
data = data[1..-1]
|
101
|
+
end
|
102
|
+
|
103
|
+
end
|
104
|
+
end
|
105
|
+
|
106
|
+
rescue EOFError => error
|
107
|
+
# Nothing to do.
|
108
|
+
rescue StandardError => error
|
109
|
+
puts error.message
|
110
|
+
puts error.backtrace.join("\n")
|
111
|
+
end
|
112
|
+
|
113
|
+
return message_type
|
114
|
+
end
|
115
|
+
|
116
|
+
|
117
|
+
def self.write_message( io, message )
|
118
|
+
io.write HEADER_MARKER.pack('CC')
|
119
|
+
io.write [ message.descriptor.type ].pack('C')
|
120
|
+
io.write message.pack
|
121
|
+
end
|
122
|
+
|
123
|
+
end
|
124
|
+
|
125
|
+
end
|
@@ -0,0 +1,30 @@
|
|
1
|
+
module Px4LogReader
|
2
|
+
|
3
|
+
class LogMessage
|
4
|
+
attr_reader :descriptor
|
5
|
+
attr_reader :fields
|
6
|
+
def initialize( descriptor, fields )
|
7
|
+
@descriptor = descriptor
|
8
|
+
@fields = fields
|
9
|
+
end
|
10
|
+
|
11
|
+
def get( index )
|
12
|
+
|
13
|
+
index = index
|
14
|
+
if index.class == String
|
15
|
+
index = descriptor.field_list[ index ]
|
16
|
+
end
|
17
|
+
|
18
|
+
return @fields[ index ]
|
19
|
+
end
|
20
|
+
|
21
|
+
def pack
|
22
|
+
return @descriptor.pack_message( @fields )
|
23
|
+
end
|
24
|
+
|
25
|
+
def to_s
|
26
|
+
return to_csv_line( Time.now )
|
27
|
+
end
|
28
|
+
end
|
29
|
+
|
30
|
+
end
|
@@ -0,0 +1,185 @@
|
|
1
|
+
|
2
|
+
module Px4LogReader
|
3
|
+
|
4
|
+
class MessageDescriptor
|
5
|
+
|
6
|
+
attr_reader :name
|
7
|
+
attr_reader :type
|
8
|
+
attr_reader :length
|
9
|
+
attr_reader :format
|
10
|
+
attr_reader :field_list
|
11
|
+
attr_reader :format_specifier
|
12
|
+
|
13
|
+
def initialize(attrs={})
|
14
|
+
if attrs.keys.uniq.sort == [:name,:type,:length,:format,:fields].uniq.sort
|
15
|
+
@name = attrs[:name]
|
16
|
+
@type = attrs[:type]
|
17
|
+
@length = attrs[:length]
|
18
|
+
@format = attrs[:format]
|
19
|
+
@format_specifier = MessageDescriptor.build_format_specifier( @format )
|
20
|
+
fields = attrs[:fields]
|
21
|
+
|
22
|
+
@field_list = MessageDescriptor.build_field_list( fields )
|
23
|
+
elsif attrs.size > 0
|
24
|
+
raise "Missing attributes"
|
25
|
+
else
|
26
|
+
@name = nil
|
27
|
+
@type = nil
|
28
|
+
@length = nil
|
29
|
+
@format = nil
|
30
|
+
|
31
|
+
@field_list = {}
|
32
|
+
end
|
33
|
+
end
|
34
|
+
|
35
|
+
def from_message( message )
|
36
|
+
|
37
|
+
if message.descriptor.type != 0x80
|
38
|
+
|
39
|
+
raise InvalidDescriptorError.new(
|
40
|
+
'Invalid descriptor type for format specifier message' )
|
41
|
+
|
42
|
+
elsif message.fields.count != 5
|
43
|
+
|
44
|
+
raise InvalidDescriptorError.new(
|
45
|
+
"Invalid field count for format specifier message: expected 5 fields, found #{message.fields.count}" )
|
46
|
+
|
47
|
+
end
|
48
|
+
|
49
|
+
@type, @length, @name, @format, fields_string = message.fields
|
50
|
+
|
51
|
+
@format_specifier = MessageDescriptor.build_format_specifier( @format )
|
52
|
+
|
53
|
+
unless fields_string.empty?
|
54
|
+
|
55
|
+
fields = fields_string.split(',')
|
56
|
+
|
57
|
+
if fields.length != @format.length
|
58
|
+
raise InvalidDescriptorError.new(
|
59
|
+
"Field count must match format length: expected #{@format.length}; found #{fields.length}")
|
60
|
+
else
|
61
|
+
@field_list = MessageDescriptor.build_field_list( fields )
|
62
|
+
end
|
63
|
+
|
64
|
+
end
|
65
|
+
|
66
|
+
end
|
67
|
+
|
68
|
+
def to_message
|
69
|
+
return LogMessage.new( FORMAT_MESSAGE, [ @type, @length, @name, @format, @fields_string ] )
|
70
|
+
end
|
71
|
+
|
72
|
+
class << self
|
73
|
+
|
74
|
+
def build_field_list( fields )
|
75
|
+
field_list = {}
|
76
|
+
fields.each_with_index do |field,index|
|
77
|
+
field_list[field] = index
|
78
|
+
end
|
79
|
+
field_list
|
80
|
+
end
|
81
|
+
|
82
|
+
def build_format_specifier( px4_format_string )
|
83
|
+
format_specifier = ''
|
84
|
+
|
85
|
+
px4_format_string.unpack('C*').each do |field_format|
|
86
|
+
case field_format
|
87
|
+
when 0x66 # 'f'
|
88
|
+
format_specifier << 'F'
|
89
|
+
when 0x71, 0x51 # 'q', 'Q'
|
90
|
+
format_specifier << 'l!'
|
91
|
+
when 0x69, 0x4C, 0x65 # 'i', 'L', 'e'
|
92
|
+
format_specifier << 'l'
|
93
|
+
when 0x49, 0x45 # 'I', 'E'
|
94
|
+
format_specifier << 'L'
|
95
|
+
when 0x62 # 'b'
|
96
|
+
format_specifier << 'c'
|
97
|
+
when 0x42, 0x4D # 'B', 'M'
|
98
|
+
format_specifier << 'C'
|
99
|
+
when 0x68, 0x63 # 'h', 'c'
|
100
|
+
format_specifier << 's'
|
101
|
+
when 0x48, 0x43 # 'H', 'C'
|
102
|
+
format_specifier << 'S'
|
103
|
+
when 0x6E # 'n'
|
104
|
+
format_specifier << 'A4'
|
105
|
+
when 0x4E # 'N'
|
106
|
+
format_specifier << 'A16'
|
107
|
+
when 0x5A # 'Z'
|
108
|
+
format_specifier << 'A64'
|
109
|
+
else
|
110
|
+
raise InvalidDescriptorError.new(
|
111
|
+
"Invalid format specifier: '#{ '0x%02X' % field_format }' in #{px4_format_string}")
|
112
|
+
end
|
113
|
+
end
|
114
|
+
|
115
|
+
return format_specifier
|
116
|
+
end
|
117
|
+
end
|
118
|
+
|
119
|
+
def unpack_message( message_data )
|
120
|
+
|
121
|
+
if @format_specifier.nil?
|
122
|
+
@format_specifier = MessageDescriptor.build_format_specifier( @format )
|
123
|
+
end
|
124
|
+
|
125
|
+
fields = message_data.unpack( @format_specifier )
|
126
|
+
|
127
|
+
@format.unpack('C*').each_with_index do |field_format,index|
|
128
|
+
case field_format
|
129
|
+
when 0x4C # 'L'
|
130
|
+
fields[index] = fields[index] * 1.0E-7
|
131
|
+
when 0x63, 0x43, 0x45 # 'c', 'C', 'E'
|
132
|
+
fields[index] = fields[index] * 1.0E-2
|
133
|
+
else
|
134
|
+
end
|
135
|
+
end
|
136
|
+
|
137
|
+
if fields.nil? || fields.empty?
|
138
|
+
raise "No fields"
|
139
|
+
end
|
140
|
+
|
141
|
+
return LogMessage.new( self, fields )
|
142
|
+
end
|
143
|
+
|
144
|
+
def pack_message( fields )
|
145
|
+
|
146
|
+
if fields.count != @format.length
|
147
|
+
raise "Descriptor format length must match message field count"
|
148
|
+
end
|
149
|
+
|
150
|
+
corrected_fields = fields.dup
|
151
|
+
@format.unpack('C*').each_with_index do |field_format,index|
|
152
|
+
case field_format
|
153
|
+
when 0x4C # 'L'
|
154
|
+
corrected_fields[index] /= 1.0E-7
|
155
|
+
when 0x63, 0x43, 0x45 # 'c', 'C', 'E'
|
156
|
+
corrected_fields[index] /= 1.0E-2
|
157
|
+
else
|
158
|
+
end
|
159
|
+
end
|
160
|
+
|
161
|
+
return corrected_fields.pack( @format_specifier )
|
162
|
+
end
|
163
|
+
|
164
|
+
def to_s
|
165
|
+
puts "#{@name}( #{@type} ):"
|
166
|
+
puts " length = #{@length}"
|
167
|
+
puts " format = #{@format}"
|
168
|
+
@field_list.each do |name,index|
|
169
|
+
puts " field#{index} = #{name}"
|
170
|
+
end
|
171
|
+
end
|
172
|
+
|
173
|
+
end
|
174
|
+
|
175
|
+
#
|
176
|
+
# Message descriptor for format messages
|
177
|
+
#
|
178
|
+
FORMAT_MESSAGE = Px4LogReader::MessageDescriptor.new({
|
179
|
+
name: 'FMT',
|
180
|
+
type: 0x80,
|
181
|
+
length: 89,
|
182
|
+
format: 'BBnNZ',
|
183
|
+
fields: [ "Type", "Length", "Name", "Format", "Labels" ] }).freeze
|
184
|
+
|
185
|
+
end
|