altoslib: Deal with new Mega log format that has 32-bit gyro cal

[fw/altos] / altoslib / AltosEepromMega.java

diff --git a/altoslib/AltosEepromMega.java b/altoslib/AltosEepromMega.java
index adaa7f3197f84a1a0d1a5a9b99518fed9bcfa53d..aaff2410ff527588686c723e1efdac26007e7566 100644 (file)
--- a/altoslib/AltosEepromMega.java
+++ b/altoslib/AltosEepromMega.java
@@ -26,6 +26,8 @@ public class AltosEepromMega extends AltosEeprom {
 
        public static final int max_sat = 12;
 
+       private int log_format;
+
        public int record_length() { return record_length; }
 
        /* AO_LOG_FLIGHT elements */
@@ -35,9 +37,36 @@ public class AltosEepromMega extends AltosEeprom {
        public int ground_accel_along() { return data16(8); }
        public int ground_accel_across() { return data16(10); }
        public int ground_accel_through() { return data16(12); }
-       public int ground_roll() { return data16(14); }
-       public int ground_pitch() { return data16(16); }
-       public int ground_yaw() { return data16(18); }
+       public int ground_roll() {
+               switch (log_format) {
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA:
+                       return data32(16);
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
+                       return data16(14);
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
+       public int ground_pitch() {
+               switch (log_format) {
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA:
+                       return data32(20);
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
+                       return data16(16);
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
+       public int ground_yaw() {
+               switch (log_format) {
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA:
+                       return data32(24);
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
+                       return data16(18);
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
 
        /* AO_LOG_STATE elements */
        public int state() { return data16(0); }
@@ -89,7 +118,8 @@ public class AltosEepromMega extends AltosEeprom {
        public int svid(int n) { return data8(2 + n * 2); }
        public int c_n(int n) { return data8(2 + n * 2 + 1); }
 
-       public AltosEepromMega (AltosEepromChunk chunk, int start) throws ParseException {
+       public AltosEepromMega (AltosEepromChunk chunk, int start, int log_format) throws ParseException {
+               this.log_format = log_format;
                parse_chunk(chunk, start);
        }
 
@@ -221,11 +251,12 @@ public class AltosEepromMega extends AltosEeprom {
                }
        }
 
-       public AltosEepromMega (String line) {
+       public AltosEepromMega (String line, int log_format) {
+               this.log_format = log_format;
                parse_string(line);
        }
 
-       static public LinkedList<AltosEeprom> read(FileInputStream input) {
+       static public LinkedList<AltosEeprom> read(FileInputStream input, int log_format) {
                LinkedList<AltosEeprom> megas = new LinkedList<AltosEeprom>();
 
                for (;;) {
@@ -234,7 +265,7 @@ public class AltosEepromMega extends AltosEeprom {
                                if (line == null)
                                        break;
                                try {
-                                       AltosEepromMega mega = new AltosEepromMega(line);
+                                       AltosEepromMega mega = new AltosEepromMega(line, log_format);
                                        if (mega.cmd != AltosLib.AO_LOG_INVALID)
                                                megas.add(mega);
                                } catch (Exception e) {