diff --git a/src/me/drton/jmavlib/log/MAVLinkLogReader.java b/src/me/drton/jmavlib/log/MAVLinkLogReader.java new file mode 100644 index 0000000..d7e291b --- /dev/null +++ b/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 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 = ""; + break; + } + fieldsFormats.put(fieldName(msg, field), type); + } + } + + private void updateInfo() throws IOException, FormatErrorException { + fieldsFormats = new HashMap(); + Set messagesSysIDs = new HashSet(); + 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 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 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 getVersion() { + return Collections.emptyMap(); + } + + @Override + public Map 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 data = new HashMap(); + 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(); + } +} diff --git a/src/me/drton/jmavlib/mavlink/MAVLinkMessage.java b/src/me/drton/jmavlib/mavlink/MAVLinkMessage.java index 436af84..135de4f 100644 --- a/src/me/drton/jmavlib/mavlink/MAVLinkMessage.java +++ b/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)); diff --git a/src/me/drton/jmavlib/mavlink/MAVLinkSchema.java b/src/me/drton/jmavlib/mavlink/MAVLinkSchema.java index 24f59d2..854c19f 100644 --- a/src/me/drton/jmavlib/mavlink/MAVLinkSchema.java +++ b/src/me/drton/jmavlib/mavlink/MAVLinkSchema.java @@ -98,6 +98,10 @@ public MAVLinkMessageDefinition getMessageDefinition(String msgName) { return definitionsByName.get(msgName); } + public Map getMessageDefinitions() { + return definitionsByName; + } + public void addMessageDefinition(MAVLinkMessageDefinition definition) { definitions[definition.id] = definition; definitionsByName.put(definition.name, definition); diff --git a/src/me/drton/jmavlib/mavlink/MAVLinkStream.java b/src/me/drton/jmavlib/mavlink/MAVLinkStream.java index cd6e3d0..f599dd9 100644 --- a/src/me/drton/jmavlib/mavlink/MAVLinkStream.java +++ b/src/me/drton/jmavlib/mavlink/MAVLinkStream.java @@ -4,18 +4,21 @@ 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(); } @@ -23,54 +26,60 @@ 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(); } } }