altos/lisp: Fix uninitialized values in ao_lisp_make_const

[fw/altos] / altoslib / AltosEepromMega.java

diff --git a/altoslib/AltosEepromMega.java b/altoslib/AltosEepromMega.java
index adaa7f3197f84a1a0d1a5a9b99518fed9bcfa53d..082d6054f2fe8b25244c0f76fa8ce0d73d09c334 100644 (file)
--- a/altoslib/AltosEepromMega.java
+++ b/altoslib/AltosEepromMega.java
@@ -3,7 +3,8 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -15,7 +16,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  */
 
-package org.altusmetrum.altoslib_5;
+package org.altusmetrum.altoslib_11;
 
 import java.io.*;
 import java.util.*;
@@ -26,6 +27,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 +38,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 +119,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);
        }
 
@@ -119,6 +150,12 @@ public class AltosEepromMega extends AltosEeprom {
                        state.set_flight(flight());
                        state.set_ground_accel(ground_accel());
                        state.set_ground_pressure(ground_pres());
+                       state.set_accel_ground(ground_accel_along(),
+                                              ground_accel_across(),
+                                              ground_accel_through());
+                       state.set_gyro_zero(ground_roll() / 512.0,
+                                           ground_pitch() / 512.0,
+                                           ground_yaw() / 512.0);
                        break;
                case AltosLib.AO_LOG_STATE:
                        state.set_tick(tick);
@@ -128,22 +165,21 @@ public class AltosEepromMega extends AltosEeprom {
                        state.set_tick(tick);
                        state.set_ms5607(pres(), temp());
 
-                       AltosIMU imu = new AltosIMU();
-                       imu.accel_x = AltosIMU.convert_accel(accel_x());
-                       imu.accel_y = AltosIMU.convert_accel(accel_y());
-                       imu.accel_z = AltosIMU.convert_accel(accel_z());
+                       AltosIMU        imu = new AltosIMU(accel_y(),   /* along */
+                                                          accel_x(),   /* across */
+                                                          accel_z(),   /* through */
+                                                          gyro_y(),    /* roll */
+                                                          gyro_x(),    /* pitch */
+                                                          gyro_z());   /* yaw */
 
-                       imu.gyro_x = AltosIMU.convert_gyro(gyro_x());
-                       imu.gyro_y = AltosIMU.convert_gyro(gyro_y());
-                       imu.gyro_z = AltosIMU.convert_gyro(gyro_z());
-                       state.imu = imu;
+                       if (log_format == AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD)
+                               state.check_imu_wrap(imu);
 
-                       AltosMag mag = new AltosMag();
-                       mag.x = AltosMag.convert_gauss(mag_x());
-                       mag.y = AltosMag.convert_gauss(mag_y());
-                       mag.z = AltosMag.convert_gauss(mag_z());
+                       state.set_imu(imu);
 
-                       state.mag = mag;
+                       state.set_mag(new AltosMag(mag_x(),
+                                                  mag_y(),
+                                                  mag_z()));
 
                        state.set_accel(accel());
 
@@ -221,11 +257,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 +271,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) {