From 06cf43cea9149f8c04c80785a613408454eab4f2 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 12 Feb 2020 20:07:02 -0800 Subject: [PATCH] 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 --- altoslib/AltosCalData.java | 24 ++- altoslib/AltosConfigData.java | 1 + altoslib/AltosEepromRecordMega.java | 26 +++ altoslib/AltosEepromRecordSet.java | 1 + altoslib/AltosIMU.java | 228 ++++++++++++++++++++----- altoslib/AltosIdleFetch.java | 46 +++-- altoslib/AltosLib.java | 1 + altoslib/AltosTelemetry.java | 3 +- altoslib/AltosTelemetryMegaSensor.java | 17 +- altoslib/AltosTelemetryStandard.java | 7 +- 10 files changed, 280 insertions(+), 74 deletions(-) diff --git a/altoslib/AltosCalData.java b/altoslib/AltosCalData.java index 22d19def..7b16ae39 100644 --- 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 04129c44..a97b996f 100644 --- 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 b61a6fa0..491e7c3d 100644 --- 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 5e2adba3..fcf224f3 100644 --- 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 8adf710e..7a5c222e 100644 --- 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 4b9a5254..7a700bdf 100644 --- 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 cfc32624..52f9af8c 100644 --- 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 a19e4226..46c96806 100644 --- 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 e97e18e2..749f6bbd 100644 --- 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 3f585cea..bef27e67 100644 --- 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); -- 2.30.2