px4_log_reader 0.0.5

checksums.yaml

@@ -0,0 +1,7 @@
+---
+SHA1:
+  metadata.gz: 4866bef24e75e23430bf53f0d79860ce51179031
+  data.tar.gz: 99c4506ee9f1324a75be0967a1e4d0e16b92410b
+SHA512:
+  metadata.gz: 4fbfd8dfa77d550d8bc7139fdae14600d50d5450d017b881b2a45c478f35fff60fa835b77cbd3697dcf5b2d6d19ce66182e0ee2204e6eb4ce21c77752aa35487
+  data.tar.gz: e5d5c1eb3fa33137568d1908c40cd68249066809c61ca0a1b360f59fe69f9ec7f1b49dadf08f802084d8ceacb6f34b83ff4eed44e188ba01aae096777df7fd8e

data/.gitignore

@@ -0,0 +1,4 @@
+*.sublime-*
+.DS_Store
+*.gem
+*.txt

data/README.md

@@ -0,0 +1,43 @@
+[![Build Status](https://travis-ci.org/rgmann/px4_log_reader.svg?branch=master)](https://travis-ci.org/rgmann/px4_log_reader)
+
+## Welcome to Px4LogReader ##
+
+Px4LogReader is a Ruby gem for parsing PX4 © self-describing log files.
+
+
+## Install the gem ##
+
+Install it with [RubyGems](https://rubygems.org/)
+
+    gem install px4_log_reader
+
+or add this to your Gemfile if you use [Bundler](http://gembundler.com/):
+
+    gem "px4_log_reader"
+
+
+## Getting Started ##
+
+### Example: Read PX4 log file ###
+
+     require 'px4_log_reader'
+
+     Px4LogReader.open( 'a_test_log.px4log' ) do |reader|
+        reader.each_message( { with: [ 'ATT' ] } ) do |message,context|
+
+           att = [ messaged.get('roll'), message.get('pitch'), message.get('yaw') ]
+
+           puts "ATT( @ #{context.find_by_name('GPS').get('time')} ): roll=#{att[0]}, pitch=#{att[1]}, yaw=#{att[2]}"
+
+        end
+     end
+
+
+## License and copyright ##
+
+Px4LogReader is released under the BSD License.
+
+Copyright: © 2016 by Robert Glissmann. All Rights Reserved.
+
+"PX4" is a copyright of PX4 Autopilot (aka PX4 Dev Team). All Rights Reserved.
+

data/Rakefile

@@ -0,0 +1,124 @@
+require 'rake/testtask'
+
+Rake::TestTask.new do |t|
+  t.libs << 'test'
+end
+
+desc "Run tests"
+task :default => :test
+
+def read_default_log_filename
+	log_filename = ''
+	File.open( 'default_log_path.txt', 'r' ) do |input|
+		log_filename = input.readline
+	end
+
+	unless File.exist? log_filename
+		raise "Failed to find '#{log_filename}'"
+	end
+
+	return log_filename
+end
+
+task :build_minor, :version do |t,args|
+
+	Dir.glob('px4_log_reader-*.gem') do |gem_file|
+		puts "Deleting '#{gem_file}'"
+		FileUtils.rm gem_file
+	end
+
+	version_file = File.join( 'lib', 'px4_log_reader', 'version.rb' )
+	current_version = '0.0.1'
+
+	if File.exist?( File.expand_path( version_file ) )
+		require_relative version_file
+		current_version = Px4LogReader::VERSION
+	end
+
+	args.with_defaults( :version => current_version.next )
+	next_version = args[:version]
+
+	File.open( version_file, 'w+' ) do |output|
+		puts "Generation '#{version_file}' for version '#{next_version}'"
+		output.write "module Px4LogReader\n\tVERSION = '#{next_version}'\nend"
+	end
+
+	`gem uninstall px4_log_reader && gem build px4_log_reader.gemspec && gem install ./px4_log_reader-#{next_version}.gem`
+end
+
+task :gen_desc_cache do
+	require 'px4_log_reader'
+
+	log_filename = read_default_log_filename
+
+	log_file = File.open( log_filename, 'r' )
+	descriptors = Px4LogReader::LogFile.read_descriptors( log_file )
+
+	cache_filename = File.join( 'test', 'test_files', 'pixhawk_descriptor_cache.px4mc' )
+	cache = Px4LogReader::MessageDescriptorCache.new( cache_filename )
+	cache.write_descriptors( descriptors )
+
+end
+
+task :dump_desc_cache do
+	require 'px4_log_reader'
+
+	cache_filename = File.join( 'test', 'test_files', 'pixhawk_descriptor_cache.px4mc' )
+	cache = Px4LogReader::MessageDescriptorCache.new( cache_filename )
+	descriptors = cache.read_descriptors
+
+	descriptors.each do |type,descriptor|
+		puts [descriptor.name,'%02X'%descriptor.type].concat(descriptor.field_list.keys).join(',')
+	end
+end
+
+task :cut_log, :count, :skip, :filename do |t,args|
+	args.with_defaults(:count => 20, :skip => 0, :filename => File.join('test','test_files','test_log.px4log'))
+
+	require 'px4_log_reader'
+
+	input_filename = read_default_log_filename
+
+	cache_filename = File.join( 'test', 'test_files', 'pixhawk_descriptor_cache.px4mc' )
+	cache = Px4LogReader::MessageDescriptorCache.new( cache_filename )
+	message_descriptors = cache.read_descriptors
+
+
+	File.open( input_filename, 'r' ) do |input|
+		File.open( args[:filename], 'wb+' ) do |output|
+
+			# Write the descriptors
+			message_descriptors.each do |type,descriptor|
+				Px4LogReader::LogFile.write_message( output, descriptor.to_message )
+			end
+
+			skip_count = args[:skip].to_i
+			skip_count.times do
+				Px4LogReader::LogFile.read_message( input, message_descriptors )
+			end
+
+			read_count = args[:count].to_i
+			read_count.times do
+				message = Px4LogReader::LogFile.read_message( input, message_descriptors )
+				Px4LogReader::LogFile.write_message( output, message )
+			end
+		end
+	end
+
+end
+
+task :dump, :filename do |t,args|
+	require 'px4_log_reader'
+
+	cache_filename = File.join( 'test', 'test_files', 'pixhawk_descriptor_cache.px4mc' )
+	cache = Px4LogReader::MessageDescriptorCache.new( cache_filename )
+	message_descriptors = cache.read_descriptors
+
+	File.open( args[:filename], 'r' ) do |input|
+		count = 1
+		while ( message = Px4LogReader::LogFile.read_message( input, message_descriptors ) ) do
+			puts "#{count}) #{message.descriptor.name}, #{'%02X'%message.descriptor.type}"
+			count += 1
+		end
+	end
+end

data/lib/px4_log_reader/invalid_descriptor_error.rb

@@ -0,0 +1,25 @@
+module Px4LogReader
+
+	class Error < StandardError
+
+		def initialize( message='' )
+			super( message )
+		end
+
+	end
+
+	class InvalidDescriptorError < Error
+
+		def initialize( message )
+			super( message )
+		end
+
+	end
+
+	class FileNotFoundError < Error
+		def initialize( filename )
+			super( "Failed to find '#{filename}'" )
+		end
+	end
+
+end

data/lib/px4_log_reader/log_buffer.rb

@@ -0,0 +1,153 @@
+module Px4LogReader
+
+	class LogBuffer
+
+		attr_reader :data
+		attr_reader :read_position
+		attr_reader :write_position
+
+		def initialize( size )
+			@data = Array.new( size, 0x00 )
+			@read_position = 0
+			@write_position = 0
+		end
+
+		def reset
+			@read_position = 0
+			@write_position = 0
+		end
+
+		def write( file )
+			while ( @write_position < @data.size ) do
+				begin
+
+					bytes = file.read( @data.size - @write_position )
+
+					if bytes
+						write_bytes( bytes.unpack('C*') )
+					else
+						break
+					end
+
+				rescue EOFError => error
+					break
+				end
+			end
+		end
+
+		def read( num_bytes )
+			data = ''
+
+			if !empty?
+				last_index = @read_position + num_bytes
+				last_index = @data.size if last_index > @data.size
+
+				read_count = last_index - @read_position
+				data = @data[ @read_position, read_count ].pack('C*')
+				@read_position += read_count
+			end
+
+			return data
+		end
+
+		def empty?
+			return ( @read_position == @write_position )
+		end
+
+		protected
+
+		def write_bytes( bytes )
+			bytes.each do |byte|
+				if @write_position < @data.size
+					@data[ @write_position ] = byte
+					@write_position += 1
+				end
+			end
+		end
+	end
+
+	class LogBufferArray
+
+		attr_reader :buffers
+		attr_reader :current_buffer_index
+
+		def initialize
+			@buffers = []
+			@active_file = nil
+			@current_buffer_index = 0
+		end
+
+		def set_file( file, options = {} )
+
+			opts = {
+				buffer_count: 2,
+				load_buffers: true,
+				buffer_size: 1024
+			}.merge( options )
+
+			@active_file = file
+			@buffer_count = opts[:buffer_count]
+			@buffer_size = opts[:buffer_size] / opts[:buffer_count]
+
+			load_buffers if opts[:load_buffers]
+		end
+
+		def load_buffers
+
+			@buffers = []
+			@buffer_count.times do
+				@buffers << LogBuffer.new( @buffer_size )
+			end
+
+			@buffers.each do |buffer|
+				buffer.write( @active_file )
+			end
+		end
+
+		def read( num_bytes )
+			data = ''
+
+			while ( data.size < num_bytes ) do
+
+				data << active_buffer.read( num_bytes )
+
+				if data.length < num_bytes
+					increment_buffer
+					break if active_buffer.empty?
+				end
+
+			end
+
+			return data
+		end
+
+		def load_empty_buffers
+			
+			inactive_buffer_index = ( @current_buffer_index + 1 ) % @buffer_count
+
+			while ( inactive_buffer_index != @current_buffer_index ) do
+				buffer = buffers[ inactive_buffer_index ]
+
+				if buffer.empty?
+					buffer.reset
+					buffer.write( @active_file )
+				end
+
+				inactive_buffer_index = ( inactive_buffer_index + 1 ) % @buffer_count
+			end
+
+		end
+
+		protected
+
+		def active_buffer
+			return @buffers[ @current_buffer_index ]
+		end
+
+		def increment_buffer
+			@current_buffer_index = (@current_buffer_index + 1) % @buffer_count
+		end
+
+	end
+
+end

data/lib/px4_log_reader/log_file.rb

@@ -0,0 +1,125 @@
+
+module Px4LogReader
+
+	module LogFile
+
+		HEADER_MARKER = [0xA3,0x95]
+	   HEADER_LENGTH = 3
+	   FORMAT_DESCRIPTOR_TABLE = { FORMAT_MESSAGE.type => FORMAT_MESSAGE }.freeze
+
+		def self.read_descriptors( buffered_io, descriptor_cache=nil )
+
+			message_descriptors = {}
+
+			while ( message_descriptor = read_descriptor( buffered_io ) ) do
+				if !message_descriptors.keys.include? message_descriptor.type
+					message_descriptors[ message_descriptor.type ] = message_descriptor
+				end
+			end
+
+			# If a cache filename was supplied, dump the descriptors to the cache
+			if descriptor_cache
+				descriptor_cache.write_descriptors( message_descriptors )
+			end
+
+			return message_descriptors
+		end
+
+		def self.read_descriptor( buffered_io, skip_corrupt=true )
+
+			message_descriptor = nil
+
+		   	begin
+
+				descriptor_message = read_message( buffered_io, FORMAT_DESCRIPTOR_TABLE )
+
+				if descriptor_message
+
+					message_descriptor = Px4LogReader::MessageDescriptor.new
+					message_descriptor.from_message( descriptor_message )
+
+				end
+
+			rescue Px4LogReader::InvalidDescriptorError => error
+
+				if skip_corrupt
+					retry
+				else
+					raise error
+				end
+
+			rescue StandardError => e
+				puts "#{e.class}: #{e.message}"
+				puts e.backtrace.join("\n")
+			end
+
+			return message_descriptor
+	   end
+
+	   def self.read_message( buffered_io, message_descriptors )
+
+			message = nil
+			while message.nil? do
+				message_type = read_message_header( buffered_io )
+
+				if message_type
+
+					message_descriptor = message_descriptors[ message_type ]
+
+					if message_descriptor
+						message_data = buffered_io.read( message_descriptor.length - HEADER_LENGTH )
+						message = message_descriptor.unpack_message( message_data )
+					end
+
+				elsif message_type.nil?
+					break
+				end
+			end
+
+			return message
+	   end
+
+	   def self.read_message_header( buffered_io )
+	      message_type = nil
+
+	      begin
+
+	         data = buffered_io.read(2)
+
+	         if data && data.length == 2
+
+	            while !data.empty? && message_type.nil? do
+
+	            	if ( byte = buffered_io.read(1) )
+		               data << byte
+		            end
+
+	               if data.unpack('CCC')[0,2] == HEADER_MARKER
+	                  message_type = data.unpack('CCC').last & 0xFF
+	               else
+	                  data = data[1..-1]
+	               end
+
+	            end
+	         end
+
+	      rescue EOFError => error
+	         # Nothing to do.
+	      rescue StandardError => error
+	         puts error.message
+	         puts error.backtrace.join("\n")
+	      end
+
+	      return message_type
+	   end
+
+
+	   def self.write_message( io, message )
+	   	io.write HEADER_MARKER.pack('CC')
+	   	io.write [ message.descriptor.type ].pack('C')
+	   	io.write message.pack
+	   end
+
+	end
+
+end

data/lib/px4_log_reader/log_message.rb

@@ -0,0 +1,30 @@
+module Px4LogReader
+
+	class LogMessage
+		attr_reader :descriptor
+		attr_reader :fields
+		def initialize( descriptor, fields )
+			@descriptor = descriptor
+			@fields     = fields
+		end
+
+		def get( index )
+
+			index = index
+			if index.class == String
+				index = descriptor.field_list[ index ]
+			end
+
+			return @fields[ index ]
+		end
+
+		def pack
+			return @descriptor.pack_message( @fields )
+		end
+
+		def to_s
+			return to_csv_line( Time.now )
+		end
+	end
+
+end

data/lib/px4_log_reader/message_descriptor.rb

@@ -0,0 +1,185 @@
+
+module Px4LogReader
+
+	class MessageDescriptor
+
+	   attr_reader :name
+	   attr_reader :type
+	   attr_reader :length
+	   attr_reader :format
+	   attr_reader :field_list
+	   attr_reader :format_specifier
+
+	   def initialize(attrs={})
+			if attrs.keys.uniq.sort == [:name,:type,:length,:format,:fields].uniq.sort
+				@name = attrs[:name]
+				@type = attrs[:type]
+				@length = attrs[:length]
+				@format = attrs[:format]
+				@format_specifier = MessageDescriptor.build_format_specifier( @format )
+				fields = attrs[:fields]
+
+				@field_list = MessageDescriptor.build_field_list( fields )
+			elsif attrs.size > 0
+				raise "Missing attributes"
+			else
+				@name = nil
+				@type = nil
+				@length = nil
+				@format = nil
+
+				@field_list = {}
+			end
+	   end
+
+		def from_message( message )
+
+			if message.descriptor.type != 0x80
+
+				raise InvalidDescriptorError.new(
+					'Invalid descriptor type for format specifier message' )
+
+			elsif message.fields.count != 5
+
+				raise InvalidDescriptorError.new(
+					"Invalid field count for format specifier message: expected 5 fields, found #{message.fields.count}" )
+
+			end
+
+			@type, @length, @name, @format, fields_string = message.fields
+
+			@format_specifier = MessageDescriptor.build_format_specifier( @format )
+
+			unless fields_string.empty?
+
+				fields = fields_string.split(',')
+
+				if fields.length != @format.length
+					raise InvalidDescriptorError.new(
+						"Field count must match format length: expected #{@format.length}; found #{fields.length}")
+				else
+					@field_list = MessageDescriptor.build_field_list( fields )
+				end
+
+			end
+			
+	   end
+
+	   def to_message
+	   	return LogMessage.new( FORMAT_MESSAGE, [ @type, @length, @name, @format, @fields_string ] )
+	   end
+
+		class << self
+
+			def build_field_list( fields )
+				field_list = {}
+				fields.each_with_index do |field,index|
+					field_list[field] = index
+				end
+				field_list
+			end
+
+			def build_format_specifier( px4_format_string )
+				format_specifier = ''
+
+				px4_format_string.unpack('C*').each do |field_format|
+					case field_format
+					when 0x66 # 'f'
+						format_specifier << 'F'
+					when 0x71, 0x51 # 'q', 'Q'
+						format_specifier << 'l!'
+					when 0x69, 0x4C, 0x65 # 'i', 'L', 'e'
+						format_specifier << 'l'
+					when 0x49, 0x45 # 'I', 'E'
+						format_specifier << 'L'
+					when 0x62 # 'b'
+						format_specifier << 'c'
+					when 0x42, 0x4D # 'B', 'M'
+						format_specifier << 'C'
+					when 0x68, 0x63 # 'h', 'c'
+						format_specifier << 's'
+					when 0x48, 0x43 # 'H', 'C'
+						format_specifier << 'S'
+					when 0x6E # 'n'
+						format_specifier << 'A4'
+					when 0x4E # 'N'
+						format_specifier << 'A16'
+					when 0x5A # 'Z'
+						format_specifier << 'A64'
+					else
+						raise InvalidDescriptorError.new(
+							"Invalid format specifier: '#{ '0x%02X' % field_format }' in #{px4_format_string}")
+					end
+				end
+
+				return format_specifier
+			end
+		end
+
+		def unpack_message( message_data )
+
+			if @format_specifier.nil?
+				@format_specifier = MessageDescriptor.build_format_specifier( @format )
+			end
+
+			fields = message_data.unpack( @format_specifier )
+
+			@format.unpack('C*').each_with_index do |field_format,index|
+				case field_format
+				when 0x4C # 'L'
+					fields[index] = fields[index] * 1.0E-7
+				when 0x63, 0x43, 0x45 # 'c', 'C', 'E'
+					fields[index] = fields[index] * 1.0E-2
+				else
+				end
+			end
+
+			if fields.nil? || fields.empty?
+				raise "No fields"
+			end
+
+			return LogMessage.new( self, fields )
+		end
+
+		def pack_message( fields )
+
+			if fields.count != @format.length
+				raise "Descriptor format length must match message field count"
+			end
+
+			corrected_fields = fields.dup
+			@format.unpack('C*').each_with_index do |field_format,index|
+				case field_format
+				when 0x4C # 'L'
+					corrected_fields[index] /= 1.0E-7
+				when 0x63, 0x43, 0x45 # 'c', 'C', 'E'
+					corrected_fields[index] /= 1.0E-2
+				else
+				end
+	   	end
+
+	   	return corrected_fields.pack( @format_specifier )
+	   end
+
+	   def to_s
+			puts "#{@name}( #{@type} ):"
+			puts "   length = #{@length}"
+			puts "   format = #{@format}"
+			@field_list.each do |name,index|
+				puts "   field#{index} = #{name}"
+			end
+		end
+
+	end
+
+	#
+	# Message descriptor for format messages
+	#
+	FORMAT_MESSAGE = Px4LogReader::MessageDescriptor.new({
+		name: 'FMT',
+		type: 0x80,
+		length: 89,
+		format: 'BBnNZ',
+		fields: [ "Type", "Length", "Name", "Format", "Labels" ] }).freeze
+
+end