From 8c92656576aff3ceab5bead3c835d96c36098fe7 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 20 Oct 2018 17:24:41 -0700 Subject: [PATCH] altoslib: Add support for EasyMega-v2 MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit adxl375 instead of mma6555. mpu9250 rotated 90° compared with other megas. Signed-off-by: Keith Packard --- altoslib/AltosConfigData.java | 2 + altoslib/AltosEepromRecordMega.java | 113 +++++++++++++++++++++++++--- altoslib/AltosEepromRecordSet.java | 1 + altoslib/AltosLib.java | 1 + 4 files changed, 108 insertions(+), 9 deletions(-) diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java index 29ce033d..d315f96a 100644 --- a/altoslib/AltosConfigData.java +++ b/altoslib/AltosConfigData.java @@ -201,6 +201,8 @@ public class AltosConfigData { case AltosLib.AO_LOG_FORMAT_TELEMEGA: case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: return 4095 - value; + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return -value; default: return AltosLib.MISSING; } diff --git a/altoslib/AltosEepromRecordMega.java b/altoslib/AltosEepromRecordMega.java index 6b28c50a..c37dbfa2 100644 --- a/altoslib/AltosEepromRecordMega.java +++ b/altoslib/AltosEepromRecordMega.java @@ -32,6 +32,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord { switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: return data32(16); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(14); @@ -43,6 +44,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord { switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: return data32(20); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(16); @@ -54,6 +56,7 @@ public class AltosEepromRecordMega extends AltosEepromRecord { switch (log_format) { case AltosLib.AO_LOG_FORMAT_TELEMEGA: case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: return data32(24); case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: return data16(18); @@ -78,6 +81,98 @@ public class AltosEepromRecordMega extends AltosEepromRecord { private int mag_x() { return data16(20); } private int mag_z() { return data16(22); } private int mag_y() { return data16(24); } + + 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: + return accel_x(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return -accel_y(); + default: + return AltosLib.MISSING; + } + } + + private int accel_along(){ + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return accel_y(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return accel_x(); + default: + return AltosLib.MISSING; + } + } + + private int accel_through() { + return accel_z(); + } + + private int gyro_pitch() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return gyro_x(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return -gyro_y(); + default: + return AltosLib.MISSING; + } + } + + private int gyro_roll() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return gyro_y(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return gyro_x(); + default: + return AltosLib.MISSING; + } + } + + private int gyro_yaw() { + return gyro_z(); + } + + private int mag_across() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return mag_x(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return -mag_y(); + default: + return AltosLib.MISSING; + } + } + + private int mag_along() { + switch (log_format) { + case AltosLib.AO_LOG_FORMAT_TELEMEGA: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_3: + case AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD: + return mag_y(); + case AltosLib.AO_LOG_FORMAT_EASYMEGA_2: + return mag_y(); + default: + return AltosLib.MISSING; + } + } + + private int mag_through() { + return mag_z(); + } + + private int accel() { return data16(26); } /* AO_LOG_TEMP_VOLT elements */ @@ -138,16 +233,16 @@ public class AltosEepromRecordMega extends AltosEepromRecord { listener.set_pressure(pt.pres); listener.set_temperature(pt.temp); - int accel_along = accel_y(); - int accel_across = accel_x(); - int accel_through = accel_z(); - int gyro_roll = gyro_y(); - int gyro_pitch = gyro_x(); - int gyro_yaw = gyro_z(); + 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_y(); - int mag_across = mag_x(); - int mag_through = mag_z(); + int mag_along = mag_along(); + int mag_across = mag_across(); + int mag_through = mag_through(); if (log_format == AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD) cal_data.check_imu_wrap(gyro_roll, gyro_pitch, gyro_yaw); diff --git a/altoslib/AltosEepromRecordSet.java b/altoslib/AltosEepromRecordSet.java index 27f81df6..e26a1e00 100644 --- a/altoslib/AltosEepromRecordSet.java +++ b/altoslib/AltosEepromRecordSet.java @@ -71,6 +71,7 @@ public class AltosEepromRecordSet implements AltosRecordSet { 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_EASYMEGA_2: record = new AltosEepromRecordMega(eeprom); break; case AltosLib.AO_LOG_FORMAT_TELEMETRUM: diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java index 8b0170ae..93567c06 100644 --- a/altoslib/AltosLib.java +++ b/altoslib/AltosLib.java @@ -374,6 +374,7 @@ public class AltosLib { public static final int AO_LOG_FORMAT_TELEFIRETWO = 13; public static final int AO_LOG_FORMAT_EASYMINI2 = 14; public static final int AO_LOG_FORMAT_TELEMEGA_3 = 15; + public static final int AO_LOG_FORMAT_EASYMEGA_2 = 16; public static final int AO_LOG_FORMAT_NONE = 127; public static boolean isspace(int c) { -- 2.30.2