MAVLinkLogReader added, MAVLinkStream API cleanup

src/me/drton/jmavlib/log/MAVLinkLogReader.java

@@ -0,0 +1,230 @@
+package me.drton.jmavlib.log;
+
+import me.drton.jmavlib.mavlink.MAVLinkField;
+import me.drton.jmavlib.mavlink.MAVLinkMessage;
+import me.drton.jmavlib.mavlink.MAVLinkSchema;
+import me.drton.jmavlib.mavlink.MAVLinkStream;
+
+import java.io.EOFException;
+import java.io.IOException;
+import java.io.RandomAccessFile;
+import java.util.*;
+
+/**
+ * User: ton Date: 25.07.14 Time: 21:43
+ */
+public class MAVLinkLogReader implements LogReader {
+    private RandomAccessFile file;
+    private Map<String, String> fieldsFormats;
+    private MAVLinkStream stream;
+    private long time;
+    private long sizeUpdates = -1;
+    private long sizeMicroseconds = -1;
+    private long startMicroseconds = -1;
+
+    public MAVLinkLogReader(String fileName, MAVLinkSchema schema) throws IOException, FormatErrorException {
+        file = new RandomAccessFile(fileName, "r");
+        stream = new MAVLinkStream(schema, file.getChannel());
+        updateInfo();
+    }
+
+    @Override
+    public void close() throws IOException {
+        file.close();
+    }
+
+    @Override
+    public boolean seek(long seekTime) throws FormatErrorException, IOException {
+        time = 0;
+        stream.position(0);
+        if (seekTime == 0) {
+            return true;
+        }
+        while (true) {
+            long pos = stream.position();
+            MAVLinkMessage msg = stream.read();
+            if (msg == null) {
+                break;
+            }
+            long t = getTime(msg);
+            if (t >= 0) {
+                if (t > seekTime) {
+                    stream.position(pos);
+                    return true;
+                } else if (t > time) {
+                    time = t;
+                }
+            }
+        }
+        return false;
+    }
+
+    private String fieldName(MAVLinkMessage msg, MAVLinkField field) {
+        return "M" + msg.systemID + ":" + msg.getMsgName() + "." + field.name;
+    }
+
+    private void addMessageFormat(MAVLinkMessage msg) {
+        for (MAVLinkField field : msg.definition.fields) {
+            String type;
+            switch (field.type) {
+                case CHAR:
+                    type = "char";
+                    break;
+                case UINT8:
+                    type = "uint8";
+                    break;
+                case INT8:
+                    type = "int8";
+                    break;
+                case UINT16:
+                    type = "uint16";
+                    break;
+                case INT16:
+                    type = "int16";
+                    break;
+                case UINT32:
+                    type = "uint32";
+                    break;
+                case INT32:
+                    type = "int32";
+                    break;
+                case UINT64:
+                    type = "uint64";
+                    break;
+                case INT64:
+                    type = "int64";
+                    break;
+                case FLOAT:
+                    type = "float";
+                    break;
+                case DOUBLE:
+                    type = "double";
+                    break;
+                default:
+                    type = "<unknown>";
+                    break;
+            }
+            fieldsFormats.put(fieldName(msg, field), type);
+        }
+    }
+
+    private void updateInfo() throws IOException, FormatErrorException {
+        fieldsFormats = new HashMap<String, String>();
+        Set<String> messagesSysIDs = new HashSet<String>();
+        seek(0);
+        long packetsNum = 0;
+        long timeStart = -1;
+        long timeEnd = -1;
+        while (true) {
+            MAVLinkMessage msg;
+            msg = stream.read();
+            if (msg == null) {
+                break;
+            }
+
+            long t = getTime(msg);
+            if (t >= 0) {
+                if (timeStart < 0) {
+                    timeStart = t;
+                }
+                if (t > timeEnd) {
+                    timeEnd = t;
+                }
+                packetsNum++;
+            }
+
+            String msgSysID = msg.systemID + ":" + msg.getMsgName();
+            if (!messagesSysIDs.contains(msgSysID)) {
+                messagesSysIDs.add(msgSysID);
+                addMessageFormat(msg);
+            }
+        }
+        startMicroseconds = timeStart;
+        sizeUpdates = packetsNum;
+        sizeMicroseconds = timeEnd - timeStart;
+        seek(0);
+    }
+
+    private long getTime(MAVLinkMessage msg) {
+        MAVLinkField field;
+        field = msg.definition.fieldsByName.get("time_usec");
+        if (field != null) {
+            return ((Number) msg.get(field)).longValue();
+        }
+        field = msg.definition.fieldsByName.get("time_boot_ms");
+        if (field != null) {
+            return ((Number) msg.get(field)).longValue() * 1000;
+        }
+        return -1;
+    }
+
+    @Override
+    public long readUpdate(Map<String, Object> update) throws IOException, FormatErrorException {
+        MAVLinkMessage msg = stream.read();
+        if (msg == null) {
+            throw new EOFException();
+        }
+        for (MAVLinkField field : msg.definition.fields) {
+            update.put(fieldName(msg, field), msg.get(field));
+            long t = getTime(msg);
+            if (t >= 0 && t > time) {
+                time = t;
+            }
+        }
+        return time;
+    }
+
+    @Override
+    public Map<String, String> getFields() {
+        return fieldsFormats;
+    }
+
+    @Override
+    public String getFormat() {
+        return "MAVLink";
+    }
+
+    @Override
+    public long getSizeUpdates() {
+        return sizeUpdates;
+    }
+
+    @Override
+    public long getStartMicroseconds() {
+        return startMicroseconds;
+    }
+
+    @Override
+    public long getSizeMicroseconds() {
+        return sizeMicroseconds;
+    }
+
+    @Override
+    public Map<String, Object> getVersion() {
+        return Collections.emptyMap();
+    }
+
+    @Override
+    public Map<String, Object> getParameters() {
+        return Collections.emptyMap();
+    }
+
+    public static void main(String[] args) throws Exception {
+        MAVLinkLogReader reader = new MAVLinkLogReader("test.mavlink", new MAVLinkSchema("common.xml"));
+        long tStart = System.currentTimeMillis();
+        Map<String, Object> data = new HashMap<String, Object>();
+        while (true) {
+            long t;
+            data.clear();
+            try {
+                t = reader.readUpdate(data);
+            } catch (EOFException e) {
+                break;
+            }
+            System.out.println(t + " " + data);
+        }
+        long tEnd = System.currentTimeMillis();
+        System.out.println(tEnd - tStart);
+        reader.close();
+    }
+}

src/me/drton/jmavlib/mavlink/MAVLinkMessage.java

@@ -11,13 +11,13 @@ public class MAVLinkMessage {
     private final static int MSG_ID_OFFSET = 5;
     public final static int DATA_OFFSET = 6;
     private final MAVLinkSchema schema;
-    private final MAVLinkMessageDefinition definition;
-    private int msgID;
+    public final MAVLinkMessageDefinition definition;
+    public final int msgID;
     private final byte[] payload;
     private final ByteBuffer payloadBB;
     private byte sequence = 0;
-    private int systemID = 0;
-    private int componentID = 0;
+    public final int systemID;
+    public final int componentID;
     private int crc = -1;
     private Charset charset = Charset.forName("latin1");
 
@@ -79,6 +79,7 @@ public MAVLinkMessage(MAVLinkSchema schema, ByteBuffer buffer)
         }
         int payloadLen = buffer.get() & 0xff;
         if (buffer.remaining() < payloadLen + 6) {
+            buffer.position(startPos);
             throw new BufferUnderflowException();
         }
         sequence = buffer.get();
@@ -93,6 +94,7 @@ public MAVLinkMessage(MAVLinkSchema schema, ByteBuffer buffer)
             throw new MAVLinkUnknownMessage(String.format("Unknown message: %s", msgID));
         }
         if (payloadLen != definition.payloadLength) {
+            buffer.position(buffer.position() + payloadLen + 2);
             throw new MAVLinkUnknownMessage(
                     String.format("Invalid payload len for msg %s (%s): %s, should be %s", definition.name, msgID,
                             payloadLen, definition.payloadLength));

src/me/drton/jmavlib/mavlink/MAVLinkSchema.java

@@ -98,6 +98,10 @@ public MAVLinkMessageDefinition getMessageDefinition(String msgName) {
         return definitionsByName.get(msgName);
     }
 
+    public Map<String, MAVLinkMessageDefinition> getMessageDefinitions() {
+        return definitionsByName;
+    }
+
     public void addMessageDefinition(MAVLinkMessageDefinition definition) {
         definitions[definition.id] = definition;
         definitionsByName.put(definition.name, definition);

src/me/drton/jmavlib/mavlink/MAVLinkStream.java

@@ -4,73 +4,82 @@
 import java.nio.BufferUnderflowException;
 import java.nio.ByteBuffer;
 import java.nio.channels.ByteChannel;
+import java.nio.channels.FileChannel;
 
 /**
  * User: ton Date: 03.06.14 Time: 12:31
  */
 public class MAVLinkStream {
     private final MAVLinkSchema schema;
+    private final ByteChannel channel;
     private byte txSeq = 0;
     private ByteBuffer buffer = ByteBuffer.allocate(8192);
     private boolean debug = false;
 
-    public MAVLinkStream(MAVLinkSchema schema) {
+    public MAVLinkStream(MAVLinkSchema schema, ByteChannel channel) {
         this.schema = schema;
+        this.channel = channel;
         buffer.flip();
     }
 
     public void setDebug(boolean debug) {
         this.debug = debug;
     }
 
-    public ByteBuffer write(MAVLinkMessage msg) throws IOException {
-        return msg.encode(txSeq++);
-    }
-
-    public void write(MAVLinkMessage msg, ByteChannel channel) throws IOException {
+    /**
+     * Write message.
+     *
+     * @param msg Message
+     * @throws IOException on IO error
+     */
+    public void write(MAVLinkMessage msg) throws IOException {
         channel.write(msg.encode(txSeq++));
     }
 
-    public MAVLinkMessage read(ByteBuffer buffer) {
-        while (buffer.remaining() >= 8) {
-            // Got header, try to parse message
-            buffer.mark();
+    /**
+     * Read message.
+     *
+     * @return MAVLink message or null if no more messages available at the moment
+     * @throws java.io.IOException on IO error
+     */
+    public MAVLinkMessage read() throws IOException {
+        while (true) {
             try {
                 return new MAVLinkMessage(schema, buffer);
             } catch (MAVLinkProtocolException e) {
                 // Message is completely corrupted, try to sync on the next byte
-                buffer.reset();
-                buffer.get();
             } catch (MAVLinkUnknownMessage mavLinkUnknownMessage) {
                 // Message looks ok but with another protocol, skip it
                 if (debug) {
                     mavLinkUnknownMessage.printStackTrace();
                 }
+            } catch (BufferUnderflowException bufferUnderflowException) {
+                // Try to refill buffer
+                buffer.compact();
+                int n = channel.read(buffer);
+                buffer.flip();
+                if (n <= 0) {
+                    return null;
+                }
             }
         }
-        return null;
     }
 
-    public MAVLinkMessage read(ByteChannel channel) throws IOException {
-        buffer.compact();
-        channel.read(buffer);
-        buffer.flip();
+    public long position() throws IOException {
+        if (channel instanceof FileChannel) {
+            return ((FileChannel) channel).position() + buffer.position() - buffer.limit();
+        } else {
+            throw new UnsupportedOperationException();
+        }
+    }
 
-        while (true) {
-            buffer.mark();
-            try {
-                return new MAVLinkMessage(schema, buffer);
-            } catch (MAVLinkProtocolException e) {
-                buffer.reset();
-                buffer.get();
-            } catch (MAVLinkUnknownMessage mavLinkUnknownMessage) {
-                // Message looks ok but with another protocol, skip it
-                if (debug) {
-                    mavLinkUnknownMessage.printStackTrace();
-                }
-            } catch (BufferUnderflowException underflow) {
-                return null;
-            }
+    public void position(long pos) throws IOException {
+        if (channel instanceof FileChannel) {
+            ((FileChannel) channel).position(pos);
+            buffer.clear();
+            buffer.flip();
+        } else {
+            throw new UnsupportedOperationException();
         }
     }
 }