From db950e97caefdd3257c1a2de3547ab88a2bd2344 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 19 Sep 2021 14:56:16 -0700 Subject: [PATCH] altoslib: Support TeleMega v5.0 Add normalized data support Add telemega idle monitor and telemetry packet support. Signed-off-by: Keith Packard --- altoslib/AltosCalData.java | 32 ++-- altoslib/AltosEepromRecordMega.java | 76 ++++++++ altoslib/AltosIMU.java | 231 +++++++++++++++---------- altoslib/AltosIdleFetch.java | 10 ++ altoslib/AltosLib.java | 8 + altoslib/AltosMag.java | 91 +++++++--- altoslib/AltosTelemetry.java | 1 + altoslib/AltosTelemetryMegaNorm.java | 90 ++++++++++ altoslib/AltosTelemetryMegaSensor.java | 32 ---- altoslib/AltosTelemetryStandard.java | 3 + altoslib/Makefile.am | 1 + 11 files changed, 415 insertions(+), 160 deletions(-) create mode 100644 altoslib/AltosTelemetryMegaNorm.java diff --git a/altoslib/AltosCalData.java b/altoslib/AltosCalData.java index 8bca9229..c90534a9 100644 --- a/altoslib/AltosCalData.java +++ b/altoslib/AltosCalData.java @@ -312,12 +312,24 @@ public class AltosCalData { return temp_gps; } - public int imu_type = AltosLib.MISSING;; + public int imu_type = AltosLib.MISSING; + + public int imu_model = AltosLib.MISSING; + + public int mag_model = AltosLib.MISSING; public void set_imu_type(int imu_type) { this.imu_type = imu_type; } + public void set_imu_model(int imu_model) { + this.imu_model = imu_model; + } + + public void set_mag_model(int mag_model) { + this.mag_model = mag_model; + } + 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) { @@ -329,15 +341,15 @@ public class AltosCalData { } public double accel_along(double counts) { - return AltosIMU.convert_accel(counts - accel_zero_along, imu_type); + return AltosIMU.convert_accel(counts - accel_zero_along, imu_type, imu_model); } public double accel_across(double counts) { - return AltosIMU.convert_accel(counts - accel_zero_across, imu_type); + return AltosIMU.convert_accel(counts - accel_zero_across, imu_type, imu_model); } public double accel_through(double counts) { - return AltosIMU.convert_accel(counts - accel_zero_through, imu_type); + return AltosIMU.convert_accel(counts - accel_zero_through, imu_type, imu_model); } public double gyro_zero_roll = AltosLib.MISSING; @@ -357,19 +369,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, imu_type); + return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_roll, imu_type, imu_model); } 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, imu_type); + return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_pitch, imu_type, imu_model); } 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, imu_type); + return AltosIMU.gyro_degrees_per_second(counts - gyro_zero_yaw, imu_type, imu_model); } private double gyro_zero_overflow(double first) { @@ -402,19 +414,19 @@ public class AltosCalData { public double mag_along(double along) { if (along == AltosLib.MISSING) return AltosLib.MISSING; - return AltosIMU.convert_gauss(along, imu_type, AltosIMU.mag_along_axis(imu_type)); + return AltosMag.convert_gauss(along, imu_type, mag_model); } public double mag_across(double across) { if (across == AltosLib.MISSING) return AltosLib.MISSING; - return AltosIMU.convert_gauss(across, imu_type, AltosIMU.mag_across_axis(imu_type)); + return AltosMag.convert_gauss(across, imu_type, mag_model); } public double mag_through(double through) { if (through == AltosLib.MISSING) return AltosLib.MISSING; - return AltosIMU.convert_gauss(through, imu_type, AltosIMU.mag_through_axis(imu_type)); + return AltosMag.convert_gauss(through, imu_type, mag_model); } public AltosCalData() { diff --git a/altoslib/AltosEepromRecordMega.java b/altoslib/AltosEepromRecordMega.java index b858e709..57de0a41 100644 --- a/altoslib/AltosEepromRecordMega.java +++ b/altoslib/AltosEepromRecordMega.java @@ -34,6 +34,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord { case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_5: return data32(16); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(14); @@ -47,6 +48,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord { case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_5: return data32(20); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(16); @@ -60,6 +62,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord { case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: case AltosLib.AO_LOG_FORMAT_TELEMEGA_4: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_5: return data32(24); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(18); @@ -85,6 +88,17 @@ public class AltosEepromRecordMega extends AltosEepromRecord { private int mag_z() { return data16(22); } private int mag_y() { return data16(24); } + /* normalized log data */ + private int norm_accel_along() { return data16(8); } + private int norm_accel_across() { return data16(10); } + private int norm_accel_through() { return data16(12); } + private int norm_gyro_roll() { return data16(14); } + private int norm_gyro_pitch() { return data16(16); } + private int norm_gyro_yaw() { return data16(18); } + private int norm_mag_along() { return data16(20); } + private int norm_mag_across() { return data16(22); } + private int norm_mag_through() { return data16(24); } + private int imu_type() { switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: @@ -101,7 +115,35 @@ public class AltosEepromRecordMega extends AltosEepromRecord { } } + private int imu_model() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA_5: + return AltosLib.model_mpu6000; + } + return AltosLib.MISSING; + } + + private boolean sensor_normalized() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA_5: + return true; + } + return false; + } + + private int mag_model() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA_5: + return AltosLib.model_mmc5983; + } + return AltosLib.MISSING; + } + private int accel_across() { + if (sensor_normalized()) { + return norm_accel_across(); + } + switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: @@ -116,6 +158,10 @@ public class AltosEepromRecordMega extends AltosEepromRecord { } private int accel_along(){ + if (sensor_normalized()) { + return norm_accel_along(); + } + switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: @@ -130,10 +176,18 @@ public class AltosEepromRecordMega extends AltosEepromRecord { } private int accel_through() { + if (sensor_normalized()) { + return norm_accel_through(); + } + return accel_z(); } private int gyro_pitch() { + if (sensor_normalized()) { + return norm_gyro_pitch(); + } + switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: @@ -149,6 +203,10 @@ public class AltosEepromRecordMega extends AltosEepromRecord { } private int gyro_roll() { + if (sensor_normalized()) { + return norm_gyro_roll(); + } + switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: @@ -163,10 +221,18 @@ public class AltosEepromRecordMega extends AltosEepromRecord { } private int gyro_yaw() { + if (sensor_normalized()) { + return norm_gyro_yaw(); + } + return gyro_z(); } private int mag_across() { + if (sensor_normalized()) { + return norm_mag_across(); + } + switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: @@ -182,6 +248,10 @@ public class AltosEepromRecordMega extends AltosEepromRecord { } private int mag_along() { + if (sensor_normalized()) { + return norm_mag_along(); + } + switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: @@ -196,6 +266,10 @@ public class AltosEepromRecordMega extends AltosEepromRecord { } private int mag_through() { + if (sensor_normalized()) { + return norm_mag_through(); + } + return mag_z(); } @@ -240,6 +314,8 @@ public class AltosEepromRecordMega extends AltosEepromRecord { AltosGPS gps; cal_data.set_imu_type(imu_type()); + cal_data.set_imu_model(imu_model()); + cal_data.set_mag_model(mag_model()); switch (cmd()) { case AltosLib.AO_LOG_FLIGHT: diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java index f3c85e9a..0ff27c75 100644 --- a/altoslib/AltosIMU.java +++ b/altoslib/AltosIMU.java @@ -26,19 +26,44 @@ public class AltosIMU implements Cloneable { public int accel_y = AltosLib.MISSING; public int accel_z = AltosLib.MISSING; + public int accel_along = AltosLib.MISSING; + public int accel_across = AltosLib.MISSING; + public int accel_through = AltosLib.MISSING; + public int gyro_x = AltosLib.MISSING; public int gyro_y = AltosLib.MISSING; public int gyro_z = AltosLib.MISSING; + public int gyro_roll = AltosLib.MISSING; + public int gyro_pitch = AltosLib.MISSING; + public int gyro_yaw = AltosLib.MISSING; + public int mag_x = AltosLib.MISSING; public int mag_y = AltosLib.MISSING; public int mag_z = AltosLib.MISSING; + public int mag_along = AltosLib.MISSING; + public int mag_across = AltosLib.MISSING; + public int mag_through = AltosLib.MISSING; + + public int imu_model = AltosLib.MISSING; + public int mag_model = AltosLib.MISSING; + public static final double counts_per_g_mpu = 2048.0; public static final double counts_per_g_bmx = 2048.0; public static final double counts_per_g_adxl = 20.5; - private static double counts_per_g(int imu_type) { + private static double counts_per_g(int imu_type, int imu_model) { + switch (imu_model) { + case AltosLib.model_mpu6000: + case AltosLib.model_mpu9250: + return counts_per_g_mpu; + case AltosLib.model_adxl375: + return counts_per_g_adxl; + case AltosLib.model_bmx160: + return counts_per_g_bmx; + } + switch (imu_type) { case imu_type_telemega_v1_v2: case imu_type_telemega_v3: @@ -50,13 +75,13 @@ public class AltosIMU implements Cloneable { return counts_per_g_bmx; case imu_type_easymotor_v2: return counts_per_g_adxl; - default: - return AltosLib.MISSING; } + + return AltosLib.MISSING; } - public static double convert_accel(double counts, int imu_type) { - double cpg = counts_per_g(imu_type); + public static double convert_accel(double counts, int imu_type, int imu_model) { + double cpg = counts_per_g(imu_type, imu_model); if (cpg == AltosLib.MISSING) return AltosLib.MISSING; return counts / cpg * AltosConvert.gravity; @@ -69,7 +94,15 @@ public class AltosIMU implements Cloneable { 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) { + private static double counts_per_degree(int imu_type, int imu_model) { + switch (imu_model) { + case AltosLib.model_mpu6000: + case AltosLib.model_mpu9250: + return counts_per_degree_mpu; + case AltosLib.model_bmx160: + return counts_per_degree_bmx; + } + switch (imu_type) { case imu_type_telemega_v1_v2: case imu_type_telemega_v3: @@ -84,66 +117,76 @@ public class AltosIMU implements Cloneable { } } - public static double gyro_degrees_per_second(double counts, int imu_type) { - double cpd = counts_per_degree(imu_type); + public static double gyro_degrees_per_second(double counts, int imu_type, int imu_model) { + double cpd = counts_per_degree(imu_type, imu_model); + if (cpd == AltosLib.MISSING) return AltosLib.MISSING; return counts / cpd; } - 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 counts_per_gauss_bmx = 100.0; /* BMX driver converts to µT */ - public static double counts_per_gauss(int imu_type, int axis) { + public static double counts_per_gauss(int imu_type, int imu_model) { + switch (imu_model) { + case AltosLib.model_mpu9250: + return counts_per_gauss_mpu; + case AltosLib.model_bmx160: + return counts_per_gauss_bmx; + } + 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: case imu_type_easytimer_v1: - return 100.0; - default: - return AltosLib.MISSING; + return counts_per_gauss_bmx; } - } - - public static double convert_gauss(double counts, int imu_type, int imu_axis) { - double cpg = counts_per_gauss(imu_type, imu_axis); - if (cpg == AltosLib.MISSING) - return AltosLib.MISSING; - return counts / cpg; + return AltosLib.MISSING; } public boolean parse_string(String line) { - if (!line.startsWith("Accel:")) - return false; + if (line.startsWith("Accel:")) { - String[] items = line.split("\\s+"); + String[] items = line.split("\\s+"); - if (items.length >= 8) { - accel_x = Integer.parseInt(items[1]); - accel_y = Integer.parseInt(items[2]); - accel_z = Integer.parseInt(items[3]); - gyro_x = Integer.parseInt(items[5]); - gyro_y = Integer.parseInt(items[6]); - gyro_z = Integer.parseInt(items[7]); + if (items.length >= 8) { + accel_x = Integer.parseInt(items[1]); + accel_y = Integer.parseInt(items[2]); + accel_z = Integer.parseInt(items[3]); + gyro_x = Integer.parseInt(items[5]); + gyro_y = Integer.parseInt(items[6]); + gyro_z = Integer.parseInt(items[7]); + } + if (items.length >= 12) { + mag_x = Integer.parseInt(items[9]); + mag_y = Integer.parseInt(items[10]); + mag_z = Integer.parseInt(items[11]); + } + return true; } - if (items.length >= 12) { - mag_x = Integer.parseInt(items[9]); - mag_y = Integer.parseInt(items[10]); - mag_z = Integer.parseInt(items[11]); + if (line.startsWith("MPU6000:")) { + String[] items = line.split("\\s+"); + + imu_model = AltosLib.model_mpu6000; + + if (items.length >= 7) { + accel_along = Integer.parseInt(items[1]); + accel_across = Integer.parseInt(items[2]); + accel_through = Integer.parseInt(items[3]); + gyro_roll = Integer.parseInt(items[4]); + gyro_pitch = Integer.parseInt(items[5]); + gyro_yaw = Integer.parseInt(items[6]); + } + return true; } - return true; + + return false; } public AltosIMU clone() { @@ -153,14 +196,26 @@ public class AltosIMU implements Cloneable { n.accel_y = accel_y; n.accel_z = accel_z; + n.accel_along = accel_along; + n.accel_across = accel_across; + n.accel_through = accel_through; + n.gyro_x = gyro_x; n.gyro_y = gyro_y; n.gyro_z = gyro_z; + n.gyro_roll = gyro_roll; + n.gyro_pitch = gyro_pitch; + n.gyro_yaw = gyro_yaw; + n.mag_x = mag_x; n.mag_y = mag_y; n.mag_z = mag_z; + n.mag_along = mag_along; + n.mag_across = mag_across; + n.mag_through = mag_through; + return n; } @@ -176,6 +231,10 @@ public class AltosIMU implements Cloneable { public static final int imu_type_easymotor_v2 = 6; /* ADXL375 (accel only) */ private int accel_across(int imu_type) { + + if (accel_across != AltosLib.MISSING) + return accel_across; + switch (imu_type) { case imu_type_telemega_v1_v2: case imu_type_telemega_v3: @@ -194,6 +253,9 @@ public class AltosIMU implements Cloneable { } private int accel_along(int imu_type) { + if (accel_along != AltosLib.MISSING) + return accel_along; + switch (imu_type) { case imu_type_telemega_v1_v2: case imu_type_telemega_v3: @@ -211,10 +273,16 @@ public class AltosIMU implements Cloneable { } private int accel_through(int imu_type) { + if (accel_through != AltosLib.MISSING) + return accel_through; + return accel_z; } private int gyro_roll(int imu_type) { + if (gyro_roll != AltosLib.MISSING) + return gyro_roll; + switch (imu_type) { case imu_type_telemega_v1_v2: case imu_type_telemega_v3: @@ -230,6 +298,9 @@ public class AltosIMU implements Cloneable { } private int gyro_pitch(int imu_type) { + if (gyro_pitch != AltosLib.MISSING) + return gyro_pitch; + switch (imu_type) { case imu_type_telemega_v1_v2: case imu_type_telemega_v3: @@ -246,25 +317,16 @@ public class AltosIMU implements Cloneable { } private int gyro_yaw(int imu_type) { - return gyro_z; - } + if (gyro_yaw != AltosLib.MISSING) + return gyro_yaw; - 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_easymega_v2: - case imu_type_telemega_v4: - case imu_type_easytimer_v1: - return imu_axis_y; - default: - return AltosLib.MISSING; - } + return gyro_z; } private int mag_across(int imu_type) { + if (mag_across != AltosLib.MISSING) + return mag_across; + switch (imu_type) { case imu_type_telemega_v1_v2: case imu_type_telemega_v3: @@ -280,22 +342,10 @@ public class AltosIMU implements Cloneable { } } - 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: - case imu_type_easytimer_v1: - return imu_axis_x; - default: - return AltosLib.MISSING; - } - } - private int mag_along(int imu_type) { + if (mag_along != AltosLib.MISSING) + return mag_along; + switch (imu_type) { case imu_type_telemega_v1_v2: case imu_type_telemega_v3: @@ -310,6 +360,13 @@ public class AltosIMU implements Cloneable { } } + private int mag_through(int imu_type) { + if (mag_through != AltosLib.MISSING) + return mag_through; + + return mag_z; + } + private static boolean is_primary_accel(int imu_type) { switch (imu_type) { case imu_type_easytimer_v1: @@ -319,23 +376,20 @@ public class AltosIMU implements Cloneable { } } - 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 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_type != AltosLib.MISSING) + cal_data.set_imu_type(imu_type); if (imu != null) { - if (imu.gyro_x != AltosLib.MISSING) { + if (imu.imu_model != AltosLib.MISSING) + cal_data.set_imu_model(imu.imu_model); + if (imu.mag_model != AltosLib.MISSING) + cal_data.set_mag_model(imu.mag_model); + + if (imu.gyro_roll(imu_type) != AltosLib.MISSING) { cal_data.set_gyro_zero(0, 0, 0); listener.set_gyro(cal_data.gyro_roll(imu.gyro_roll(imu_type)), cal_data.gyro_pitch(imu.gyro_pitch(imu_type)), @@ -355,7 +409,7 @@ public class AltosIMU implements Cloneable { accel = -accel; listener.set_acceleration(cal_data.acceleration(accel)); } - if (imu.mag_x != AltosLib.MISSING) { + if (imu.mag_along(imu_type) != AltosLib.MISSING) { 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))); @@ -366,17 +420,6 @@ public class AltosIMU implements Cloneable { } public AltosIMU() { - accel_x = AltosLib.MISSING; - accel_y = AltosLib.MISSING; - accel_z = AltosLib.MISSING; - - gyro_x = AltosLib.MISSING; - gyro_y = AltosLib.MISSING; - gyro_z = AltosLib.MISSING; - - mag_x = AltosLib.MISSING; - mag_y = AltosLib.MISSING; - mag_z = AltosLib.MISSING; } public AltosIMU(AltosLink link) throws InterruptedException, TimeoutException { diff --git a/altoslib/AltosIdleFetch.java b/altoslib/AltosIdleFetch.java index c4a32788..b2fd3c6f 100644 --- a/altoslib/AltosIdleFetch.java +++ b/altoslib/AltosIdleFetch.java @@ -39,6 +39,7 @@ class AltosIdler { static final int idle_ms5607 = 9; static final int idle_adxl375 = 10; static final int idle_adxl375_easymotor_v2 = 11; + static final int idle_imu = 12; static final int idle_sensor_tm = 100; static final int idle_sensor_metrum = 101; @@ -76,6 +77,9 @@ class AltosIdler { case idle_imu_et_v1: AltosIMU.provide_data(listener, link, AltosIMU.imu_type_easytimer_v1); break; + case idle_imu: + AltosIMU.provide_data(listener, link, AltosLib.MISSING); + break; case idle_mag: AltosMag.provide_data(listener, link); break; @@ -208,6 +212,12 @@ public class AltosIdleFetch implements AltosDataProvider { AltosIdler.idle_ms5607, AltosIdler.idle_imu_tm_v4, AltosIdler.idle_sensor_mega), + new AltosIdler("TeleMega-v5", + AltosIdler.idle_gps, + AltosIdler.idle_adxl375, + AltosIdler.idle_ms5607, + AltosIdler.idle_imu, AltosIdler.idle_mag, + AltosIdler.idle_sensor_mega), new AltosIdler("EasyMega-v1", AltosIdler.idle_mma655x, AltosIdler.idle_ms5607, diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index cf1fa1ed..07ed31e1 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -385,8 +385,16 @@ public class AltosLib { 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_EASYMOTOR = 20; + public static final int AO_LOG_FORMAT_TELEMEGA_5 = 21; public static final int AO_LOG_FORMAT_NONE = 127; + public static final int model_mpu6000 = 0; + public static final int model_mpu9250 = 1; + public static final int model_adxl375 = 2; + public static final int model_bmx160 = 3; + public static final int model_hmc5883 = 4; + public static final int model_mmc5983 = 5; + public static boolean isspace(int c) { switch (c) { case ' ': diff --git a/altoslib/AltosMag.java b/altoslib/AltosMag.java index ab5d3f57..c7f1ac1d 100644 --- a/altoslib/AltosMag.java +++ b/altoslib/AltosMag.java @@ -22,44 +22,84 @@ import java.util.concurrent.*; import java.io.*; public class AltosMag implements Cloneable { - public int x; - public int z; - public int y; + public int along = AltosLib.MISSING; + public int across = AltosLib.MISSING; + public int through = AltosLib.MISSING; - public static final double counts_per_gauss = 1090; + public static final int model_hmc5883 = 0; + public static final int model_mmc5983 = 1; - public static double convert_gauss(double counts) { - return counts / counts_per_gauss; + public int mag_model = AltosLib.MISSING; + + public static final double counts_per_gauss_hmc5883 = 1090; + public static final double counts_per_gauss_mmc5983 = 4096; + + public static double counts_per_gauss(int imu_type, int mag_model) { + switch(mag_model) { + case AltosLib.model_hmc5883: + return counts_per_gauss_hmc5883; + case AltosLib.model_mmc5983: + return counts_per_gauss_mmc5983; + } + switch (imu_type) { + case AltosIMU.imu_type_telemega_v1_v2: + case AltosIMU.imu_type_easymega_v1: + return counts_per_gauss_hmc5883; + } + + return AltosIMU.counts_per_gauss(imu_type, mag_model); + } + + public static double convert_gauss(double counts, int imu_type, int mag_model) { + double cpg = counts_per_gauss(imu_type, mag_model); + + if (cpg == AltosLib.MISSING) + return AltosLib.MISSING; + + return counts / cpg; } public boolean parse_string(String line) { - if (!line.startsWith("X:")) - return false; + if (line.startsWith("X:")) { - String[] items = line.split("\\s+"); + String[] items = line.split("\\s+"); - if (items.length >= 6) { - x = Integer.parseInt(items[1]); - z = Integer.parseInt(items[3]); - y = Integer.parseInt(items[5]); + mag_model = model_hmc5883; + + if (items.length >= 6) { + across = Integer.parseInt(items[1]); + through = Integer.parseInt(items[3]); + along = Integer.parseInt(items[5]); + } + return true; + } + if (line.startsWith("MMC5983:")) { + String[] items = line.split("\\s+"); + + mag_model = model_mmc5983; + + if (items.length >= 4) { + along = Integer.parseInt(items[1]); + across = Integer.parseInt(items[2]); + through = Integer.parseInt(items[3]); + } + return true; } - return true; + + return false; } public AltosMag clone() { AltosMag n = new AltosMag(); - n.x = x; - n.z = z; - n.y = y; + n.along = along; + n.across = across; + n.through = through; return n; } public AltosMag() { - x = AltosLib.MISSING; - z = AltosLib.MISSING; - y = AltosLib.MISSING; } static public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException { @@ -67,10 +107,13 @@ public class AltosMag implements Cloneable { AltosMag mag = new AltosMag(link); AltosCalData cal_data = listener.cal_data(); - if (mag != null) - listener.set_mag(cal_data.mag_along(mag.y), - cal_data.mag_across(mag.x), - cal_data.mag_through(mag.z)); + if (mag != null) { + if (mag.mag_model != AltosLib.MISSING) + cal_data.set_mag_model(mag.mag_model); + listener.set_mag(cal_data.mag_along(mag.along), + cal_data.mag_across(mag.across), + cal_data.mag_through(mag.through)); + } } catch (TimeoutException te) { } } diff --git a/altoslib/AltosTelemetry.java b/altoslib/AltosTelemetry.java index ec3d8495..54dd04da 100644 --- a/altoslib/AltosTelemetry.java +++ b/altoslib/AltosTelemetry.java @@ -81,6 +81,7 @@ public abstract class AltosTelemetry implements AltosDataProvider { final static int packet_type_mini2 = 0x10; final static int packet_type_mini3 = 0x11; final static int packet_type_mega_sensor_bmx160 = 0x12; + final static int packet_type_mega_norm_mpu6000_mmc5983 = 0x13; static AltosTelemetry parse_hex(String hex) throws ParseException, AltosCRCException { AltosTelemetry telem = null; diff --git a/altoslib/AltosTelemetryMegaNorm.java b/altoslib/AltosTelemetryMegaNorm.java new file mode 100644 index 00000000..4a8bf106 --- /dev/null +++ b/altoslib/AltosTelemetryMegaNorm.java @@ -0,0 +1,90 @@ +/* + * Copyright © 2021 Keith Packard + * + * 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; 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 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +package org.altusmetrum.altoslib_14; + +public class AltosTelemetryMegaNorm extends AltosTelemetryStandard { + int orient() { return int8(5); } + + int accel() { return int16(6); } + int pres() { return int32(8); } + int temp() { return int16(12); } + + int accel_along() { return int16(14); } + int accel_across() { return int16(16); } + int accel_through() { return int16(18); } + + int gyro_roll() { return int16(20); } + int gyro_pitch() { return int16(22); } + int gyro_yaw() { return int16(24); } + + int mag_along() { return int16(26); } + int mag_across() { return int16(28); } + int mag_through() { return int16(30); } + + int imu_model, mag_model; + + public AltosTelemetryMegaNorm(int[] bytes, int imu_model, int mag_model) throws AltosCRCException { + super(bytes); + this.imu_model = imu_model; + this.mag_model = mag_model; + } + + public void provide_data(AltosDataListener listener) { + super.provide_data(listener); + + AltosCalData cal_data = listener.cal_data(); + + listener.set_acceleration(cal_data.acceleration(accel())); + listener.set_pressure(pres()); + listener.set_temperature(temp() / 100.0); + + listener.set_orient(orient()); + cal_data.set_imu_model(imu_model); + cal_data.set_mag_model(mag_model); + + /* XXX we have no calibration data for these values */ + + if (cal_data.accel_zero_along == AltosLib.MISSING) + cal_data.set_accel_zero(0, 0, 0); + if (cal_data.gyro_zero_roll == AltosLib.MISSING) + cal_data.set_gyro_zero(0, 0, 0); + + int accel_along = accel_along(); + int accel_across = accel_across(); + int accel_through = accel_through(); + + int gyro_roll = gyro_roll(); + int gyro_pitch = gyro_pitch(); + int gyro_yaw = gyro_yaw(); + + int mag_along = mag_along(); + int mag_across = mag_across(); + int mag_through = mag_through(); + + listener.set_accel(cal_data.accel_along(accel_along), + cal_data.accel_across(accel_across), + cal_data.accel_through(accel_through)); + listener.set_gyro(cal_data.gyro_roll(gyro_roll), + cal_data.gyro_pitch(gyro_pitch), + cal_data.gyro_yaw(gyro_yaw)); + listener.set_mag(cal_data.mag_along(mag_along), + cal_data.mag_across(mag_across), + cal_data.mag_through(mag_through)); + } +} diff --git a/altoslib/AltosTelemetryMegaSensor.java b/altoslib/AltosTelemetryMegaSensor.java index cda977eb..6acbb5bc 100644 --- a/altoslib/AltosTelemetryMegaSensor.java +++ b/altoslib/AltosTelemetryMegaSensor.java @@ -105,20 +105,6 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard { return gyro_z(); } - public static int mag_across_axis(int imu_type) { - switch (imu_type) { - case AltosIMU.imu_type_telemega_v1_v2: - case AltosIMU.imu_type_telemega_v3: - case AltosIMU.imu_type_easymega_v1: - return AltosIMU.imu_axis_x; - case AltosIMU.imu_type_telemega_v4: - case AltosIMU.imu_type_easymega_v2: - return AltosIMU.imu_axis_y; - default: - return AltosLib.MISSING; - } - } - private int mag_across(int imu_type) { switch (imu_type) { case AltosIMU.imu_type_telemega_v1_v2: @@ -133,20 +119,6 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard { } } - public static int mag_along_axis(int imu_type) { - switch (imu_type) { - case AltosIMU.imu_type_telemega_v1_v2: - case AltosIMU.imu_type_telemega_v3: - case AltosIMU.imu_type_easymega_v1: - return AltosIMU.imu_axis_y; - case AltosIMU.imu_type_easymega_v2: - case AltosIMU.imu_type_telemega_v4: - return AltosIMU.imu_axis_x; - default: - return AltosLib.MISSING; - } - } - private int mag_along(int imu_type) { switch (imu_type) { case AltosIMU.imu_type_telemega_v1_v2: @@ -161,10 +133,6 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard { } } - public static int mag_through_axis(int imu_type) { - return AltosIMU.imu_axis_z; - } - private int mag_through(int imu_type) { return mag_z(); } diff --git a/altoslib/AltosTelemetryStandard.java b/altoslib/AltosTelemetryStandard.java index b8e5d3d6..7f6626db 100644 --- a/altoslib/AltosTelemetryStandard.java +++ b/altoslib/AltosTelemetryStandard.java @@ -96,6 +96,9 @@ public abstract class AltosTelemetryStandard extends AltosTelemetry { case packet_type_mini3: telem = new AltosTelemetryMini3(bytes); break; + case packet_type_mega_norm_mpu6000_mmc5983: + telem = new AltosTelemetryMegaNorm(bytes, AltosLib.model_mpu6000, AltosLib.model_mmc5983); + break; default: telem = new AltosTelemetryRaw(bytes); break; diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am index 9c8c6691..39e287fa 100644 --- a/altoslib/Makefile.am +++ b/altoslib/Makefile.am @@ -127,6 +127,7 @@ altoslib_JAVA = \ AltosTelemetryLocation.java \ AltosTelemetryMap.java \ AltosTelemetryMegaSensor.java \ + AltosTelemetryMegaNorm.java \ AltosTelemetryMegaData.java \ AltosTelemetryMini2.java \ AltosTelemetryMini3.java \ -- 2.30.2