altoslib: Add support for TeleMega v4

TeleMega v4 has a new IMU chip, which required adding support for
multiple IMU types, including different scale factors for each mag
sensor axis.

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/altoslib/AltosCalData.java b/altoslib/AltosCalData.java
index 22d19def1ba9397fbdec3ab510131407afe8bdf8..7b16ae3911abb45ab30f767542dec8ddd80d33b5 100644 (file)
--- a/altoslib/AltosCalData.java
+++ b/altoslib/AltosCalData.java
@@ -301,6 +301,12 @@ public class AltosCalData {
                return temp_gps;
        }
 
+       public int      imu_type = AltosLib.MISSING;;
+
+       public void set_imu_type(int imu_type) {
+               this.imu_type = imu_type;
+       }
+
        public double   accel_zero_along, accel_zero_across, accel_zero_through;
 
        public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
@@ -312,15 +318,15 @@ public class AltosCalData {
        }
 
        public double accel_along(double counts) {
-               return AltosIMU.convert_accel(counts - accel_zero_along);
+               return AltosIMU.convert_accel(counts - accel_zero_along, imu_type);
        }
 
        public double accel_across(double counts) {
-               return AltosIMU.convert_accel(counts - accel_zero_across);
+               return AltosIMU.convert_accel(counts - accel_zero_across, imu_type);
        }
 
        public double accel_through(double counts) {
-               return AltosIMU.convert_accel(counts - accel_zero_through);
+               return AltosIMU.convert_accel(counts - accel_zero_through, imu_type);
        }
 
        public double   gyro_zero_roll, gyro_zero_pitch, gyro_zero_yaw;
@@ -338,19 +344,19 @@ public class AltosCalData {
                if (gyro_zero_roll == AltosLib.MISSING || counts == AltosLib.MISSING)
                        return AltosLib.MISSING;
 
-               return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_roll);
+               return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_roll, imu_type);
        }
 
        public double gyro_pitch(double counts) {
                if (gyro_zero_pitch == AltosLib.MISSING || counts == AltosLib.MISSING)
                        return AltosLib.MISSING;
-               return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_pitch);
+               return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_pitch, imu_type);
        }
 
        public double gyro_yaw(double counts) {
                if (gyro_zero_yaw == AltosLib.MISSING || counts == AltosLib.MISSING)
                        return AltosLib.MISSING;
-               return AltosIMU.gyro_degrees_per_second(counts, gyro_zero_yaw);
+               return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_yaw, imu_type);
        }
 
        private double gyro_zero_overflow(double first) {
@@ -383,19 +389,19 @@ public class AltosCalData {
        public double mag_along(double along) {
                if (along == AltosLib.MISSING)
                        return AltosLib.MISSING;
-               return AltosMag.convert_gauss(along);
+               return AltosIMU.convert_gauss(along, imu_type, AltosIMU.mag_along_axis(imu_type));
        }
 
        public double mag_across(double across) {
                if (across == AltosLib.MISSING)
                        return AltosLib.MISSING;
-               return AltosMag.convert_gauss(across);
+               return AltosIMU.convert_gauss(across, imu_type, AltosIMU.mag_across_axis(imu_type));
        }
 
        public double mag_through(double through) {
                if (through == AltosLib.MISSING)
                        return AltosLib.MISSING;
-               return AltosMag.convert_gauss(through);
+               return AltosIMU.convert_gauss(through, imu_type, AltosIMU.mag_through_axis(imu_type));
        }
 
        public AltosCalData() {

diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java
index 04129c44d456d901d7449fd3d505a521aded2471..a97b996fa89800ba1000791196d933817ff32bc1 100644 (file)
--- a/altoslib/AltosConfigData.java
+++ b/altoslib/AltosConfigData.java
@@ -200,6 +200,7 @@ public class AltosConfigData {
                case AltosLib.AO_LOG_FORMAT_TELEMETRUM:
                case AltosLib.AO_LOG_FORMAT_TELEMEGA:
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return 4095 - value;
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
                        return -value;

diff --git a/altoslib/AltosEepromRecordMega.java b/altoslib/AltosEepromRecordMega.java
index b61a6fa0f6d30178b589446b8335b951101473aa..491e7c3d611f71ced0314ecb9679b1346d593cb1 100644 (file)
--- a/altoslib/AltosEepromRecordMega.java
+++ b/altoslib/AltosEepromRecordMega.java
@@ -33,6 +33,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
                case AltosLib.AO_LOG_FORMAT_TELEMEGA:
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return data32(16);
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
                        return data16(14);
@@ -45,6 +46,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
                case AltosLib.AO_LOG_FORMAT_TELEMEGA:
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return data32(20);
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
                        return data16(16);
@@ -57,6 +59,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
                case AltosLib.AO_LOG_FORMAT_TELEMEGA:
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return data32(24);
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
                        return data16(18);
@@ -82,11 +85,28 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
        private int mag_z() { return data16(22); }
        private int mag_y() { return data16(24); }
 
+       private int imu_type() {
+               switch (log_format) {
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
+                       return AltosIMU.imu_type_telemega_v1_v2;
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
+                       return AltosIMU.imu_type_telemega_v3;
+               case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
+                       return AltosIMU.imu_type_easymega_v2;
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
+                       return AltosIMU.imu_type_telemega_v4;
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
+
        private int accel_across() {
                switch (log_format) {
                case AltosLib.AO_LOG_FORMAT_TELEMEGA:
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return accel_x();
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
                        return -accel_y();
@@ -102,6 +122,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
                        return accel_y();
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return accel_x();
                default:
                        return AltosLib.MISSING;
@@ -132,6 +153,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
                        return gyro_y();
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return gyro_x();
                default:
                        return AltosLib.MISSING;
@@ -149,6 +171,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
                        return mag_x();
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return -mag_y();
                default:
                        return AltosLib.MISSING;
@@ -162,6 +185,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
                        return mag_y();
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        return mag_x();
                default:
                        return AltosLib.MISSING;
@@ -212,6 +236,8 @@ public class AltosEepromRecordMega extends AltosEepromRecord {
 
                AltosGPS        gps;
 
+               cal_data.set_imu_type(imu_type());
+
                switch (cmd()) {
                case AltosLib.AO_LOG_FLIGHT:
                        cal_data.set_flight(flight());

diff --git a/altoslib/AltosEepromRecordSet.java b/altoslib/AltosEepromRecordSet.java
index 5e2adba34c00ae9cf4e4a9915f52d8b531d75252..fcf224f35c2a948a3dda802ee5dafc135ff13848 100644 (file)
--- a/altoslib/AltosEepromRecordSet.java
+++ b/altoslib/AltosEepromRecordSet.java
@@ -77,6 +77,7 @@ public class AltosEepromRecordSet implements AltosRecordSet {
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_3:
                case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD:
                case AltosLib.AO_LOG_FORMAT_EASYMEGA_2:
+               case AltosLib.AO_LOG_FORMAT_TELEMEGA_4:
                        record = new AltosEepromRecordMega(eeprom);
                        break;
                case AltosLib.AO_LOG_FORMAT_TELEMETRUM:

diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java
index 8adf710e028a03f8e401ec4c48de5c1e9138a805..7a5c222e1df43e675e119c6b0443cc990721da48 100644 (file)
--- a/altoslib/AltosIMU.java
+++ b/altoslib/AltosIMU.java
@@ -34,18 +34,90 @@ public class AltosIMU implements Cloneable {
        public int              mag_y = AltosLib.MISSING;
        public int              mag_z = AltosLib.MISSING;
 
-       public static final double      counts_per_g = 2048.0;
+       public static final double      counts_per_g_mpu = 2048.0;
+       public static final double      counts_per_g_bmx = 2048.0;
+
+       private static double counts_per_g(int imu_type) {
+               switch (imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v1:
+               case imu_type_easymega_v2:
+                       return counts_per_g_mpu;
+               case  imu_type_telemega_v4:
+                       return counts_per_g_bmx;
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
+
+       public static double convert_accel(double counts, int imu_type) {
+               return counts / counts_per_g(imu_type) * AltosConvert.gravity;
+       }
+
+       public static final double      GYRO_FULLSCALE_DEGREES_MPU = 2000.0;
+       public static final double      GYRO_COUNTS_MPU = 32767.0;
+       public static final double      counts_per_degree_mpu = GYRO_COUNTS_MPU / GYRO_FULLSCALE_DEGREES_MPU;
+       public static final double      GYRO_FULLSCALE_DEGREES_BMX = 2000.0;
+       public static final double      GYRO_COUNTS_BMX = 32767.0;
+       public static final double      counts_per_degree_bmx = GYRO_COUNTS_BMX / GYRO_FULLSCALE_DEGREES_BMX;
+
+       private static double counts_per_degree(int imu_type) {
+               switch (imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v1:
+               case imu_type_easymega_v2:
+                       return counts_per_degree_mpu;
+               case  imu_type_telemega_v4:
+                       return counts_per_degree_bmx;
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
 
-       public static double convert_accel(double counts) {
-               return counts / counts_per_g * AltosConvert.gravity;
+       public static double gyro_degrees_per_second(double counts, int imu_type) {
+               return counts / counts_per_degree(imu_type);
        }
 
-       /* In radians */
-       public static final double      GYRO_FULLSCALE_DEGREES = 2000.0;
-       public static final double      GYRO_COUNTS = 32767.0;
+       public static final int imu_axis_x = 0;
+       public static final int imu_axis_y = 1;
+       public static final int imu_axis_z = 2;
+
+       public static final double MAG_FULLSCALE_GAUSS_MPU = 48.00;     /* 4800µT */
+       public static final double MAG_COUNTS_MPU = 32767.0;
+       public static final double counts_per_gauss_mpu = MAG_COUNTS_MPU / MAG_FULLSCALE_GAUSS_MPU;
+
+       public static final double MAG_FULLSCALE_GAUSS_BMX_XY = 11.50;  /* 1150µT */
+       public static final double MAG_FULLSCALE_GAUSS_BMX_Z = 25.00;   /* 2500µT */
+       public static final double MAG_COUNTS_BMX = 32767.0;
+       public static final double counts_per_gauss_bmx_xy = MAG_COUNTS_BMX / MAG_FULLSCALE_GAUSS_BMX_XY;
+       public static final double counts_per_gauss_bmx_z = MAG_COUNTS_BMX / MAG_FULLSCALE_GAUSS_BMX_Z;
+
+       public static double counts_per_gauss(int imu_type, int axis) {
+               switch(imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_easymega_v1:
+                       return AltosMag.counts_per_gauss;
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v2:
+                       return counts_per_gauss_mpu;
+               case imu_type_telemega_v4:
+                       switch (axis) {
+                       case imu_axis_x:
+                       case imu_axis_y:
+                               return counts_per_gauss_bmx_xy;
+                       case imu_axis_z:
+                               return counts_per_gauss_bmx_z;
+                       }
+                       /* fall through */
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
 
-       public static double gyro_degrees_per_second(double counts, double cal) {
-               return (counts - cal) * GYRO_FULLSCALE_DEGREES / GYRO_COUNTS;
+       public static double convert_gauss(double counts, int imu_type, int imu_axis) {
+               return counts / counts_per_gauss(imu_type, imu_axis);
        }
 
        public boolean parse_string(String line) {
@@ -54,6 +126,8 @@ public class AltosIMU implements Cloneable {
 
                String[] items = line.split("\\s+");
 
+               System.out.printf("length %d\n", items.length);
+
                if (items.length >= 8) {
                        accel_x = Integer.parseInt(items[1]);
                        accel_y = Integer.parseInt(items[2]);
@@ -88,103 +162,163 @@ public class AltosIMU implements Cloneable {
                return n;
        }
 
-       public static final int orient_telemega = 0;
-       public static final int orient_easymega_v2 = 1;
+       public static final int imu_type_telemega_v1_v2 = 0;    /* MPU6000 */
+       public static final int imu_type_telemega_v3 = 1;       /* MPU9250 */
+       public static final int imu_type_telemega_v4 = 2;       /* BMX160 */
+
+       public static final int imu_type_easymega_v1 = 3;       /* MPU6000 */
+       public static final int imu_type_easymega_v2 = 4;       /* MPU9250 */
 
-       private int accel_across(int orient) {
-               switch (orient) {
-               case orient_telemega:
+       private int accel_across(int imu_type) {
+               switch (imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v1:
                        return accel_x;
-               case orient_easymega_v2:
+               case imu_type_easymega_v2:
+                       return -accel_y;
+               case  imu_type_telemega_v4:
                        return -accel_y;
                default:
                        return AltosLib.MISSING;
                }
        }
 
-       private int accel_along(int orient) {
-               switch (orient) {
-               case orient_telemega:
+       private int accel_along(int imu_type) {
+               switch (imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v1:
                        return accel_y;
-               case orient_easymega_v2:
+               case imu_type_easymega_v2:
+               case imu_type_telemega_v4:
                        return accel_x;
                default:
                        return AltosLib.MISSING;
                }
        }
 
-       private int accel_through(int orient) {
+       private int accel_through(int imu_type) {
                return accel_z;
        }
 
-       private int gyro_roll(int orient) {
-               switch (orient) {
-               case orient_telemega:
+       private int gyro_roll(int imu_type) {
+               switch (imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v1:
                        return gyro_y;
-               case orient_easymega_v2:
+               case imu_type_easymega_v2:
+               case imu_type_telemega_v4:
                        return gyro_x;
                default:
                        return AltosLib.MISSING;
                }
        }
 
-       private int gyro_pitch(int orient) {
-               switch (orient) {
-               case orient_telemega:
+       private int gyro_pitch(int imu_type) {
+               switch (imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v1:
                        return gyro_x;
-               case orient_easymega_v2:
+               case imu_type_easymega_v2:
+               case imu_type_telemega_v4:
                        return -gyro_y;
                default:
                        return AltosLib.MISSING;
                }
        }
 
-       private int gyro_yaw(int orient) {
+       private int gyro_yaw(int imu_type) {
                return gyro_z;
        }
 
-       private int mag_across(int orient) {
-               switch (orient) {
-               case orient_telemega:
+       public static int mag_across_axis(int imu_type) {
+               switch (imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v1:
+                       return imu_axis_x;
+               case imu_type_telemega_v4:
+               case imu_type_easymega_v2:
+                       return imu_axis_y;
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
+
+       private int mag_across(int imu_type) {
+               switch (imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v1:
                        return mag_x;
-               case orient_easymega_v2:
+               case imu_type_telemega_v4:
+               case imu_type_easymega_v2:
                        return -mag_y;
                default:
                        return AltosLib.MISSING;
                }
        }
 
-       private int mag_along(int orient) {
-               switch (orient) {
-               case orient_telemega:
+       public static int mag_along_axis(int imu_type) {
+               switch (imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v1:
+                       return imu_axis_y;
+               case imu_type_easymega_v2:
+               case imu_type_telemega_v4:
+                       return imu_axis_x;
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
+
+       private int mag_along(int imu_type) {
+               switch (imu_type) {
+               case imu_type_telemega_v1_v2:
+               case imu_type_telemega_v3:
+               case imu_type_easymega_v1:
                        return mag_y;
-               case orient_easymega_v2:
+               case imu_type_easymega_v2:
+               case imu_type_telemega_v4:
                        return mag_x;
                default:
                        return AltosLib.MISSING;
                }
        }
 
-       private int mag_through(int orient) {
+       public static int mag_through_axis(int imu_type) {
+               return imu_axis_z;
+       }
+
+       private int mag_through(int imu_type) {
                return mag_z;
        }
 
-       static public void provide_data(AltosDataListener listener, AltosLink link, int orient) throws InterruptedException {
+       static public void provide_data(AltosDataListener listener, AltosLink link, int imu_type) throws InterruptedException {
                try {
                        AltosIMU        imu = new AltosIMU(link);
                        AltosCalData    cal_data = listener.cal_data();
 
+                       cal_data.set_imu_type(imu_type);
+
                        if (imu != null) {
-                               listener.set_gyro(cal_data.gyro_roll(imu.gyro_roll(orient)),
-                                                 cal_data.gyro_pitch(imu.gyro_pitch(orient)),
-                                                 cal_data.gyro_yaw(imu.gyro_yaw(orient)));
-                               listener.set_accel_ground(imu.accel_along(orient),
-                                                         imu.accel_across(orient),
-                                                         imu.accel_through(orient));
+                               listener.set_gyro(cal_data.gyro_roll(imu.gyro_roll(imu_type)),
+                                                 cal_data.gyro_pitch(imu.gyro_pitch(imu_type)),
+                                                 cal_data.gyro_yaw(imu.gyro_yaw(imu_type)));
+                               listener.set_accel_ground(cal_data.accel_along(imu.accel_along(imu_type)),
+                                                         cal_data.accel_across(imu.accel_across(imu_type)),
+                                                         cal_data.accel_through(imu.accel_through(imu_type)));
+                               listener.set_accel(cal_data.accel_along(imu.accel_along(imu_type)),
+                                                  cal_data.accel_across(imu.accel_across(imu_type)),
+                                                  cal_data.accel_through(imu.accel_through(imu_type)));
                                if (imu.mag_x != AltosLib.MISSING) {
-                                       listener.set_mag(cal_data.mag_along(imu.mag_along(orient)),
-                                                        cal_data.mag_across(imu.mag_across(orient)),
-                                                        cal_data.mag_through(imu.mag_through(orient)));
+                                       listener.set_mag(cal_data.mag_along(imu.mag_along(imu_type)),
+                                                        cal_data.mag_across(imu.mag_across(imu_type)),
+                                                        cal_data.mag_through(imu.mag_through(imu_type)));
                                }
                        }
                } catch (TimeoutException te) {

diff --git a/altoslib/AltosIdleFetch.java b/altoslib/AltosIdleFetch.java
index 4b9a52542241c1cd3a6f1908bb842c80fd97fbe6..7a700bdf74823f47150a51b242207ccd4bc5cc3b 100644 (file)
--- a/altoslib/AltosIdleFetch.java
+++ b/altoslib/AltosIdleFetch.java
@@ -28,12 +28,15 @@ class AltosIdler {
        int[]   idlers;
 
        static final int        idle_gps = 0;
-       static final int        idle_imu = 1;
-       static final int        idle_imu_em_v2 = 2;
-       static final int        idle_mag = 3;
-       static final int        idle_mma655x = 4;
-       static final int        idle_ms5607 = 5;
-       static final int        idle_adxl375 = 6;
+       static final int        idle_imu_tm_v1_v2 = 1;
+       static final int        idle_imu_tm_v3 = 2;
+       static final int        idle_imu_tm_v4 = 3;
+       static final int        idle_imu_em_v1 = 4;
+       static final int        idle_imu_em_v2 = 5;
+       static final int        idle_mag = 6;
+       static final int        idle_mma655x = 7;
+       static final int        idle_ms5607 = 8;
+       static final int        idle_adxl375 = 9;
 
        static final int        idle_sensor_tm = 10;
        static final int        idle_sensor_metrum = 11;
@@ -51,11 +54,20 @@ class AltosIdler {
                        case idle_gps:
                                AltosGPS.provide_data(listener, link);
                                break;
-                       case idle_imu:
-                               AltosIMU.provide_data(listener, link, AltosIMU.orient_telemega);
+                       case idle_imu_tm_v1_v2:
+                               AltosIMU.provide_data(listener, link, AltosIMU.imu_type_telemega_v1_v2);
+                               break;
+                       case idle_imu_tm_v3:
+                               AltosIMU.provide_data(listener, link, AltosIMU.imu_type_telemega_v3);
+                               break;
+                       case idle_imu_tm_v4:
+                               AltosIMU.provide_data(listener, link, AltosIMU.imu_type_telemega_v4);
+                               break;
+                       case idle_imu_em_v1:
+                               AltosIMU.provide_data(listener, link, AltosIMU.imu_type_easymega_v1);
                                break;
                        case idle_imu_em_v2:
-                               AltosIMU.provide_data(listener, link, AltosIMU.orient_easymega_v2);
+                               AltosIMU.provide_data(listener, link, AltosIMU.imu_type_easymega_v2);
                                break;
                        case idle_mag:
                                AltosMag.provide_data(listener, link);
@@ -154,30 +166,36 @@ public class AltosIdleFetch implements AltosDataProvider {
                               AltosIdler.idle_gps,
                               AltosIdler.idle_mma655x,
                               AltosIdler.idle_ms5607,
-                              AltosIdler.idle_imu, AltosIdler.idle_mag,
+                              AltosIdler.idle_imu_tm_v1_v2, AltosIdler.idle_mag,
                               AltosIdler.idle_sensor_mega),
                new AltosIdler("TeleMega-v1",
                               AltosIdler.idle_gps,
                               AltosIdler.idle_mma655x,
                               AltosIdler.idle_ms5607,
-                              AltosIdler.idle_imu, AltosIdler.idle_mag,
+                              AltosIdler.idle_imu_tm_v1_v2, AltosIdler.idle_mag,
                               AltosIdler.idle_sensor_mega),
                new AltosIdler("TeleMega-v2",
                               AltosIdler.idle_gps,
                               AltosIdler.idle_mma655x,
                               AltosIdler.idle_ms5607,
-                              AltosIdler.idle_imu, AltosIdler.idle_mag,
+                              AltosIdler.idle_imu_tm_v1_v2, AltosIdler.idle_mag,
                               AltosIdler.idle_sensor_mega),
                new AltosIdler("TeleMega-v3",
                               AltosIdler.idle_gps,
                               AltosIdler.idle_mma655x,
                               AltosIdler.idle_ms5607,
-                              AltosIdler.idle_imu,
+                              AltosIdler.idle_imu_tm_v3,
+                              AltosIdler.idle_sensor_mega),
+               new AltosIdler("TeleMega-v4",
+                              AltosIdler.idle_gps,
+                              AltosIdler.idle_adxl375,
+                              AltosIdler.idle_ms5607,
+                              AltosIdler.idle_imu_tm_v4,
                               AltosIdler.idle_sensor_mega),
                new AltosIdler("EasyMega-v1",
                               AltosIdler.idle_mma655x,
                               AltosIdler.idle_ms5607,
-                              AltosIdler.idle_imu, AltosIdler.idle_mag,
+                              AltosIdler.idle_imu_em_v1, AltosIdler.idle_mag,
                               AltosIdler.idle_sensor_mega),
                new AltosIdler("EasyMega-v2",
                               AltosIdler.idle_adxl375,

diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java
index cfc326245049e5eb04758433c97f26c1465a7a70..52f9af8c8ff1b8eab91b0f4ce6e684c0069cb285 100644 (file)
--- a/altoslib/AltosLib.java
+++ b/altoslib/AltosLib.java
@@ -377,6 +377,7 @@ public class AltosLib {
        public static final int AO_LOG_FORMAT_EASYMEGA_2 = 16;
        public static final int AO_LOG_FORMAT_TELESTATIC = 17;
        public static final int AO_LOG_FORMAT_MICROPEAK2 = 18;
+       public static final int AO_LOG_FORMAT_TELEMEGA_4 = 19;
        public static final int AO_LOG_FORMAT_NONE = 127;
 
        public static boolean isspace(int c) {

diff --git a/altoslib/AltosTelemetry.java b/altoslib/AltosTelemetry.java
index a19e4226160a7e7d561a8c36a003191b26789419..46c968061c9e38ef563f60ba82feace2461f0d94 100644 (file)
--- a/altoslib/AltosTelemetry.java
+++ b/altoslib/AltosTelemetry.java
@@ -74,12 +74,13 @@ public abstract class AltosTelemetry implements AltosDataProvider {
        final static int packet_type_location = 0x05;
        final static int packet_type_satellite = 0x06;
        final static int packet_type_companion = 0x07;
-       final static int packet_type_mega_sensor = 0x08;
+       final static int packet_type_mega_sensor_mpu = 0x08;
        final static int packet_type_mega_data = 0x09;
        final static int packet_type_metrum_sensor = 0x0a;
        final static int packet_type_metrum_data = 0x0b;
        final static int packet_type_mini2 = 0x10;
        final static int packet_type_mini3 = 0x11;
+       final static int packet_type_mega_sensor_bmx160 = 0x12;
 
        static AltosTelemetry parse_hex(String hex)  throws ParseException, AltosCRCException {
                AltosTelemetry  telem = null;

diff --git a/altoslib/AltosTelemetryMegaSensor.java b/altoslib/AltosTelemetryMegaSensor.java
index e97e18e2872da723093e5fbf4704d1dd55102e08..749f6bbd30fe9e41bfd4803e4480368ee42695a7 100644 (file)
--- a/altoslib/AltosTelemetryMegaSensor.java
+++ b/altoslib/AltosTelemetryMegaSensor.java
@@ -37,8 +37,22 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
        int     mag_z() { return int16(28); }
        int     mag_y() { return int16(30); }
 
-       public AltosTelemetryMegaSensor(int[] bytes) throws AltosCRCException {
+       int imu_type;
+
+       public AltosTelemetryMegaSensor(int[] bytes, int imu_type) throws AltosCRCException {
                super(bytes);
+               switch (imu_type) {
+               case AltosIMU.imu_type_telemega_v1_v2:
+               case AltosIMU.imu_type_telemega_v3:
+                       if (serial() < 3000)
+                               imu_type = AltosIMU.imu_type_telemega_v1_v2;
+                       else
+                               imu_type = AltosIMU.imu_type_telemega_v3;
+                       break;
+               default:
+                       break;
+               }
+               this.imu_type = imu_type;
        }
 
        public void provide_data(AltosDataListener listener) {
@@ -51,6 +65,7 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
                listener.set_temperature(temp() / 100.0);
 
                listener.set_orient(orient());
+               cal_data.set_imu_type(imu_type);
 
                /* XXX we have no calibration data for these values */
 

diff --git a/altoslib/AltosTelemetryStandard.java b/altoslib/AltosTelemetryStandard.java
index 3f585ceafb49f5721ceef61907231342fc1a4a12..bef27e670482f0f178907248632a651d5fb10450 100644 (file)
--- a/altoslib/AltosTelemetryStandard.java
+++ b/altoslib/AltosTelemetryStandard.java
@@ -75,8 +75,11 @@ public abstract class AltosTelemetryStandard extends AltosTelemetry {
                case packet_type_companion:
                        telem = new AltosTelemetryCompanion(bytes);
                        break;
-               case packet_type_mega_sensor:
-                       telem = new AltosTelemetryMegaSensor(bytes);
+               case packet_type_mega_sensor_mpu:
+                       telem = new AltosTelemetryMegaSensor(bytes, AltosIMU.imu_type_telemega_v3);
+                       break;
+               case packet_type_mega_sensor_bmx160:
+                       telem = new AltosTelemetryMegaSensor(bytes, AltosIMU.imu_type_telemega_v4);
                        break;
                case packet_type_mega_data:
                        telem = new AltosTelemetryMegaData(bytes);