diff --git a/README.md b/README.md
index 5d7e788..ac15eba 100644
--- a/README.md
+++ b/README.md
@@ -2,17 +2,17 @@
MicroAvia Java libraries.
#### Modules List
- - jmalib-log: MicroAvia logs parsing (MicroAvia ULog v2)
+ - jmalib-log: MicroAvia logs parsing (MicroAvia ULog v1 and v2)
Based on deprecated [jMAVlib](https://github.com/DrTon/jMAVlib) project with some major improvements. Added Maven support.
-Log parsing library in a typical scenario has ~100x speed up compared to original jMAVlib parser.
+Log parser in a typical scenario has ~50x speed up compared to original jMAVlib parser.
#### Using with Maven
```
com.microavia
- jmalib
+ jmalib-log
0.1.0-SNAPSHOT
diff --git a/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/AbstractParser.java b/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/AbstractParser.java
index b4bff1a..0a0dbfb 100644
--- a/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/AbstractParser.java
+++ b/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/AbstractParser.java
@@ -1,5 +1,6 @@
package com.microavia.jmalib.log.ulog;
+import com.microavia.jmalib.log.FormatErrorException;
import com.microavia.jmalib.log.ValueParser;
abstract class AbstractParser implements ValueParser {
@@ -10,7 +11,7 @@ abstract class AbstractParser implements ValueParser {
this.context = context;
}
- private static AbstractParser createFromTypeString(LogParserContext context, String typeString) {
+ private static AbstractParser createFromTypeString(LogParserContext context, String typeString) throws FormatErrorException {
FieldParser field = FieldParser.create(context, typeString);
if (field != null) {
return field;
@@ -19,10 +20,10 @@ private static AbstractParser createFromTypeString(LogParserContext context, Str
if (struct != null) {
return struct.clone();
}
- throw new RuntimeException("Unsupported type: " + typeString);
+ throw new FormatErrorException("Unsupported type: " + typeString);
}
- static AbstractParser createFromFormatString(LogParserContext context, String formatStr) {
+ static AbstractParser createFromFormatString(LogParserContext context, String formatStr) throws FormatErrorException {
if (formatStr.contains("[")) {
// Array
String[] q = formatStr.split("\\[");
diff --git a/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/StructParser.java b/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/StructParser.java
index e3e1291..d813001 100644
--- a/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/StructParser.java
+++ b/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/StructParser.java
@@ -1,5 +1,7 @@
package com.microavia.jmalib.log.ulog;
+import com.microavia.jmalib.log.FormatErrorException;
+
import java.nio.ByteBuffer;
import java.util.LinkedHashMap;
import java.util.Map;
@@ -13,7 +15,7 @@ private StructParser(LogParserContext context, LinkedHashMap 1) {
String[] fieldDescrs = formatStr.split(";");
diff --git a/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/ULogReader.java b/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/ULogReader.java
index eb9ee8c..16db852 100644
--- a/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/ULogReader.java
+++ b/jmalib-log/src/main/java/com/microavia/jmalib/log/ulog/ULogReader.java
@@ -15,8 +15,8 @@
*/
public class ULogReader extends BinaryLogReader {
private static final byte SYNC_BYTE = (byte) '>';
- private static final byte MESSAGE_TYPE_FORMAT = (byte) 'F';
- private static final byte MESSAGE_TYPE_ADD = (byte) 'A';
+ private static final byte MESSAGE_TYPE_STRUCT = (byte) 'F';
+ private static final byte MESSAGE_TYPE_TOPIC = (byte) 'A';
private static final byte MESSAGE_TYPE_DATA = (byte) 'D';
private static final byte MESSAGE_TYPE_INFO = (byte) 'I';
private static final byte MESSAGE_TYPE_PARAMETER = (byte) 'P';
@@ -31,13 +31,14 @@ public class ULogReader extends BinaryLogReader {
private long sizeUpdates = -1;
private long sizeMicroseconds = -1;
private long startMicroseconds = -1;
- private long timeLast = -1;
+ private long timeLast = Long.MIN_VALUE;
private long utcTimeReference = -1;
private Map version = new HashMap<>();
private Map parameters = new HashMap<>();
private List errors = new ArrayList<>();
private int logVersion = 0;
private int headerSize = 4;
+ private int msgDataTimestampOffset = 3;
private LogParserContext context = new LogParserContext();
public ULogReader(String fileName) throws IOException, FormatErrorException {
@@ -46,8 +47,9 @@ public ULogReader(String fileName) throws IOException, FormatErrorException {
}
public static void main(String[] args) throws Exception {
- ULogReader reader = new ULogReader("test.ulg");
+ ULogReader reader = new ULogReader("test_long_v1.ulg");
reader.addSubscription("ATTITUDE_POSITION.alt_baro");
+ reader.seek(0);
long tStart = System.currentTimeMillis();
try {
while (true) {
@@ -120,13 +122,18 @@ private void updateStatistics() throws IOException, FormatErrorException {
if (logVersionStr.startsWith("ULG")) {
logVersion = Integer.parseInt(logVersionStr.substring(3));
headerSize = 4;
+ if (logVersion >= 2) {
+ msgDataTimestampOffset = 3;
+ } else {
+ msgDataTimestampOffset = 2;
+ }
} else {
throw new FormatErrorException("Unsupported file format");
}
startMicroseconds = -1;
- timeLast = -1;
sizeUpdates = 0;
fieldsList = new HashMap<>();
+ timeLast = Long.MIN_VALUE;
try {
while (true) {
readMessage(this::handleHeaderMessage);
@@ -189,9 +196,10 @@ public void removeAllSubscriptions() {
@Override
public long readUpdate() throws IOException {
updatedSubscriptions.clear();
+ timeLast = Long.MIN_VALUE;
do {
readMessage(this::handleDataMessage);
- } while (timeLast < 0);
+ } while (timeLast == Long.MIN_VALUE);
return timeLast;
}
@@ -203,6 +211,7 @@ public Set getUpdatedSubscriptions() {
@Override
public boolean seek(long seekTime) throws IOException {
position(dataStart);
+ timeLast = Long.MIN_VALUE;
if (seekTime == 0) { // Seek to start of log
return true;
}
@@ -211,9 +220,11 @@ public boolean seek(long seekTime) throws IOException {
while (timeLast < seekTime) {
readMessage((pos, msgType, msgSize) -> {
if (msgType == MESSAGE_TYPE_DATA) {
- timeLast = buffer.getLong(buffer.position() + 3);
+ timeLast = buffer.getLong(buffer.position() + msgDataTimestampOffset);
if (timeLast >= seekTime) {
- return; // Dont consume message with timestamp > seekTime
+ // Time found, reset buffer to start of the message
+ position(pos);
+ return;
}
}
buffer.position(buffer.position() + msgSize);
@@ -268,7 +279,7 @@ private void handleHeaderMessage(long pos, int msgType, int msgSize) throws IOEx
if (dataStart == 0) {
dataStart = pos;
}
- long timestamp = buffer.getLong(buffer.position() + 2);
+ long timestamp = buffer.getLong(buffer.position() + msgDataTimestampOffset);
if (startMicroseconds < 0) {
startMicroseconds = timestamp;
}
@@ -277,22 +288,44 @@ private void handleHeaderMessage(long pos, int msgType, int msgSize) throws IOEx
buffer.position(buffer.position() + msgSize);
break;
}
- case MESSAGE_TYPE_FORMAT: {
- String[] descr = getString(buffer, msgSize).split(":");
- String name = descr[0];
- if (descr.length > 1) {
- StructParser struct = new StructParser(context, descr[1]);
- System.out.printf("STRUCT %s: %s\n", name, struct);
- context.getStructs().put(name, struct);
+ case MESSAGE_TYPE_STRUCT: {
+ if (logVersion <= 1) {
+ int msgId = buffer.get() & 0xFF;
+ int formatLen = buffer.getShort() & 0xFFFF;
+ String[] descr = getString(buffer, formatLen).split(":");
+ String name = descr[0];
+ if (descr.length > 1) {
+ StructParser struct = null;
+ try {
+ struct = new StructParser(context, descr[1]);
+ } catch (FormatErrorException e) {
+ errors.add(new FormatErrorException(pos, "Error parsing type definition", e));
+ break;
+ }
+ context.getStructs().put(name, struct);
+ topicByName.put(name, new Topic(name, struct, msgId));
+ }
+ } else {
+ String[] descr = getString(buffer, msgSize).split(":");
+ String name = descr[0];
+ if (descr.length > 1) {
+ StructParser struct = null;
+ try {
+ struct = new StructParser(context, descr[1]);
+ } catch (Exception e) {
+ errors.add(new FormatErrorException(pos, "Error parsing struct", e));
+ break;
+ }
+ context.getStructs().put(name, struct);
+ }
}
break;
}
- case MESSAGE_TYPE_ADD: {
+ case MESSAGE_TYPE_TOPIC: {
int msgId = buffer.getShort() & 0xFFFF;
String[] descr = getString(buffer, msgSize - 2).split(":");
String name = descr[0];
String structName = descr[1];
- System.out.printf("ADD: %s:%s\n", name, structName);
StructParser struct = context.getStructs().get(structName);
Topic topic = new Topic(name, struct, msgId);
topicByName.put(name, topic);
@@ -303,7 +336,13 @@ private void handleHeaderMessage(long pos, int msgType, int msgSize) throws IOEx
int keyLen = buffer.get() & 0xFF;
String[] keyDescr = getString(buffer, keyLen).split(" ");
String key = keyDescr[1];
- AbstractParser field = AbstractParser.createFromFormatString(context, keyDescr[0]);
+ AbstractParser field = null;
+ try {
+ field = AbstractParser.createFromFormatString(context, keyDescr[0]);
+ } catch (FormatErrorException e) {
+ errors.add(new FormatErrorException(pos, "Error parsing info", e));
+ break;
+ }
Object value = field.parse(buffer);
buffer.position(buffer.position() + field.size());
switch (key) {
@@ -329,7 +368,13 @@ private void handleHeaderMessage(long pos, int msgType, int msgSize) throws IOEx
int keyLen = buffer.get() & 0xFF;
String[] keyDescr = getString(buffer, keyLen).split(" ");
String key = keyDescr[1];
- AbstractParser field = AbstractParser.createFromFormatString(context, keyDescr[0]);
+ AbstractParser field = null;
+ try {
+ field = AbstractParser.createFromFormatString(context, keyDescr[0]);
+ } catch (FormatErrorException e) {
+ errors.add(new FormatErrorException(pos, "Error parsing parameter", e));
+ break;
+ }
Object value = field.parse(buffer);
buffer.position(buffer.position() + field.size());
parameters.put(key, value);
@@ -367,9 +412,8 @@ private void addFieldsToList(String path, AbstractParser value) {
private void handleDataMessage(long pos, int msgType, int msgSize) {
int bp = buffer.position();
- timeLast = -1;
if (msgType == MESSAGE_TYPE_DATA) {
- int msgId = buffer.getShort() & 0xFFFF;
+ int msgId = logVersion >= 2 ? (buffer.getShort() & 0xFFFF) : (buffer.get() & 0xFF);
List subs = subscriptions.get(msgId);
if (subs != null) {
int multiId = buffer.get() & 0xFF;
diff --git a/jmalib-math/pom.xml b/jmalib-math/pom.xml
new file mode 100644
index 0000000..71ad66e
--- /dev/null
+++ b/jmalib-math/pom.xml
@@ -0,0 +1,13 @@
+
+
+
+ jmalib
+ com.microavia
+ 0.1.0-SNAPSHOT
+
+ 4.0.0
+
+ jmalib-math
+
\ No newline at end of file
diff --git a/jmalib-math/src/main/java/com/microavia/jmalib/math/GlobalPositionProjector.java b/jmalib-math/src/main/java/com/microavia/jmalib/math/GlobalPositionProjector.java
new file mode 100644
index 0000000..510a81b
--- /dev/null
+++ b/jmalib-math/src/main/java/com/microavia/jmalib/math/GlobalPositionProjector.java
@@ -0,0 +1,71 @@
+package com.microavia.jmalib.math;
+
+import static java.lang.Math.*;
+
+/**
+ * User: ton Date: 11.07.13 Time: 22:11
+ */
+public class GlobalPositionProjector {
+ private static double r_earth = 6371000.0;
+ private boolean inited;
+ private double lat0;
+ private double lon0;
+ private double alt0;
+ private double cos_lat0;
+ private double sin_lat0;
+
+ public void reset() {
+ inited = false;
+ }
+
+ public boolean isInited() {
+ return inited;
+ }
+
+ public void init(LatLonAlt ref) {
+ inited = true;
+ lat0 = ref.lat * PI / 180.0;
+ lon0 = ref.lon * PI / 180.0;
+ alt0 = ref.alt;
+ cos_lat0 = cos(lat0);
+ sin_lat0 = sin(lat0);
+ }
+
+ public double[] project(LatLonAlt p) {
+ if (!inited) {
+ throw new RuntimeException("Not initialized");
+ }
+ double lat_rad = p.lat * PI / 180.0;
+ double lon_rad = p.lon * PI / 180.0;
+ double sin_lat = sin(lat_rad);
+ double cos_lat = cos(lat_rad);
+ double cos_d_lon = cos(lon_rad - lon0);
+ double c = acos(sin_lat0 * sin_lat + cos_lat0 * cos_lat * cos_d_lon);
+ double k = (c == 0.0) ? 1.0 : (c / sin(c));
+ double y = k * cos_lat * sin(lon_rad - lon0) * r_earth;
+ double x = k * (cos_lat0 * sin_lat - sin_lat0 * cos_lat * cos_d_lon) * r_earth;
+ double z = alt0 - p.alt;
+ return new double[]{x, y, z};
+ }
+
+ public LatLonAlt reproject(double[] v) {
+ if (!inited) {
+ throw new RuntimeException("Not initialized");
+ }
+ double x_rad = v[0] / r_earth;
+ double y_rad = v[1] / r_earth;
+ double c = sqrt(x_rad * x_rad + y_rad * y_rad);
+ double sin_c = sin(c);
+ double cos_c = cos(c);
+ double lat_rad;
+ double lon_rad;
+ if (c != 0.0) {
+ lat_rad = asin(cos_c * sin_lat0 + (x_rad * sin_c * cos_lat0) / c);
+ lon_rad = (lon0 + atan2(y_rad * sin_c, c * cos_lat0 * cos_c - x_rad * sin_lat0 * sin_c));
+ } else {
+ lat_rad = lat0;
+ lon_rad = lon0;
+ }
+ return new LatLonAlt(lat_rad * 180.0 / Math.PI, lon_rad * 180.0 / Math.PI, alt0 - v[2]);
+ }
+}
diff --git a/jmalib-math/src/main/java/com/microavia/jmalib/math/LatLonAlt.java b/jmalib-math/src/main/java/com/microavia/jmalib/math/LatLonAlt.java
new file mode 100644
index 0000000..3d3b4bc
--- /dev/null
+++ b/jmalib-math/src/main/java/com/microavia/jmalib/math/LatLonAlt.java
@@ -0,0 +1,56 @@
+package com.microavia.jmalib.math;
+
+/**
+ * User: ton Date: 05.05.14 Time: 8:41
+ */
+public class LatLonAlt {
+ public static double RADIUS_OF_EARTH = 6371000.0;
+
+ public final double lat;
+ public final double lon;
+ public final double alt;
+
+ public LatLonAlt(double lat, double lon, double alt) {
+ this.lat = lat;
+ this.lon = lon;
+ this.alt = alt;
+ }
+
+ public LatLonAlt add(double[] vector) {
+ return new LatLonAlt(
+ lat + vector[0] / RADIUS_OF_EARTH * 180.0 / Math.PI,
+ lon + vector[1] / (Math.cos(lat * Math.PI / 180.0) * RADIUS_OF_EARTH) * 180.0 / Math.PI,
+ alt - vector[2]);
+ }
+
+ public double[] sub(LatLonAlt pos) {
+ return new double[]{
+ (lat - pos.lat) * Math.PI / 180.0 * RADIUS_OF_EARTH,
+ (lon - pos.lon) * Math.PI / 180.0 * Math.cos(lat * Math.PI / 180.0) * RADIUS_OF_EARTH,
+ pos.alt - alt
+ };
+ }
+
+ public boolean isFinite() {
+ return Double.isFinite(lat) && Double.isFinite(lon) && Double.isFinite(alt);
+ }
+
+ @Override
+ public LatLonAlt clone() {
+ return new LatLonAlt(lat, lon, alt);
+ }
+
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof LatLonAlt) {
+ LatLonAlt other = (LatLonAlt) obj;
+ return (lat == other.lat) && (lon == other.lon) && (alt == other.alt);
+ }
+ return false;
+ }
+
+ @Override
+ public String toString() {
+ return String.format("lat=%s lon=%s alt=%s", lat, lon, alt);
+ }
+}
diff --git a/jmalib-math/src/main/java/com/microavia/jmalib/math/RotationUtil.java b/jmalib-math/src/main/java/com/microavia/jmalib/math/RotationUtil.java
new file mode 100644
index 0000000..109821a
--- /dev/null
+++ b/jmalib-math/src/main/java/com/microavia/jmalib/math/RotationUtil.java
@@ -0,0 +1,49 @@
+package com.microavia.jmalib.math;
+
+import static java.lang.Math.cos;
+import static java.lang.Math.sin;
+
+/**
+ * User: ton Date: 02.06.13 Time: 20:20
+ */
+public class RotationUtil {
+ public static double[] rotationMatrixByEulerAngles(double roll, double pitch, double yaw) {
+ return new double[]{
+ cos(pitch) * cos(yaw),
+ sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw),
+ cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw),
+ cos(pitch) * sin(yaw),
+ sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw),
+ cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw),
+ -sin(pitch),
+ sin(roll) * cos(pitch),
+ cos(roll) * cos(pitch)
+ };
+ }
+
+ public static double[] rotationMatrixByQuaternion(double[] q) {
+ double aSq = q[0] * q[0];
+ double bSq = q[1] * q[1];
+ double cSq = q[2] * q[2];
+ double dSq = q[3] * q[3];
+ return new double[]{
+ aSq + bSq - cSq - dSq,
+ 2.0f * (q[1] * q[2] - q[0] * q[3]),
+ 2.0f * (q[0] * q[2] + q[1] * q[3]),
+ 2.0f * (q[1] * q[2] + q[0] * q[3]),
+ aSq - bSq + cSq - dSq,
+ 2.0f * (q[2] * q[3] - q[0] * q[1]),
+ 2.0f * (q[1] * q[3] - q[0] * q[2]),
+ 2.0f * (q[0] * q[1] + q[2] * q[3]),
+ aSq - bSq - cSq + dSq
+ };
+ }
+
+ public static double[] eulerAnglesByQuaternion(double[] q) {
+ return new double[]{
+ Math.atan2(2.0 * (q[0] * q[1] + q[2] * q[3]), 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])),
+ Math.asin(2 * (q[0] * q[2] - q[3] * q[1])),
+ Math.atan2(2.0 * (q[0] * q[3] + q[1] * q[2]), 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3])),
+ };
+ }
+}
diff --git a/pom.xml b/pom.xml
index 487f2ae..120ac25 100644
--- a/pom.xml
+++ b/pom.xml
@@ -10,6 +10,7 @@
0.1.0-SNAPSHOT
jmalib-log
+ jmalib-math