altoslib: Add EasyMotor EEPROM support

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

diff --git a/altoslib/AltosEepromRecordMotor.java b/altoslib/AltosEepromRecordMotor.java
new file mode 100644 (file)
index 0000000..656feb9
--- /dev/null
+++ b/altoslib/AltosEepromRecordMotor.java
@@ -0,0 +1,101 @@
+/*
+ * Copyright © 2020 Keith Packard <keithp@keithp.com>
+ *
+ * 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.
+ */
+
+package org.altusmetrum.altoslib_14;
+
+public class AltosEepromRecordMotor extends AltosEepromRecord {
+       public static final int record_length = 16;
+
+       private int log_format;
+
+       /* AO_LOG_FLIGHT elements */
+       private int flight() { return data16(0); }
+       private int ground_accel() { return data16(2); }
+       private int ground_accel_along() { return data16(4); }
+       private int ground_accel_across() { return data16(6); }
+       private int ground_accel_through() { return data16(8); }
+
+       /* AO_LOG_STATE elements */
+       private int state() { return data16(0); }
+       private int reason() { return data16(2); }
+
+       /* AO_LOG_SENSOR elements */
+       private int motor_pres() { return data16(0); }
+       private int v_batt() { return data16(2); }
+       private int accel() { return data16(4); }
+       private int accel_across() { return data16(6); }
+       private int accel_along() { return data16(8); }
+       private int accel_through() { return data16(10); }
+
+       private int imu_type() {
+               switch (log_format) {
+               case AltosLib.AO_LOG_FORMAT_EASYMOTOR:
+                       return AltosIMU.imu_type_easymotor_v2;
+               default:
+                       return AltosLib.MISSING;
+               }
+       }
+
+       public void provide_data(AltosDataListener listener, AltosCalData cal_data) {
+               super.provide_data(listener, cal_data);
+
+               cal_data.set_imu_type(imu_type());
+
+               switch (cmd()) {
+               case AltosLib.AO_LOG_FLIGHT:
+                       cal_data.set_flight(flight());
+                       cal_data.set_ground_accel(ground_accel());
+                       listener.set_accel_ground(cal_data.accel_along(ground_accel_along()),
+                                                 cal_data.accel_across(ground_accel_across()),
+                                                 cal_data.accel_through(ground_accel_through()));
+                       break;
+               case AltosLib.AO_LOG_STATE:
+                       listener.set_state(state());
+                       break;
+               case AltosLib.AO_LOG_SENSOR:
+                       AltosConfigData config_data = eeprom.config_data();
+
+                       listener.set_battery_voltage(AltosConvert.easy_mini_2_voltage(v_batt()));
+                       double pa = AltosConvert.easy_motor_2_motor_pressure(motor_pres());
+                       listener.set_motor_pressure(pa);
+
+                       int     accel_along = accel_along();
+                       int     accel_across = accel_across();
+                       int     accel_through = accel_through();
+
+                       listener.set_accel(cal_data.accel_along(accel_along),
+                                          cal_data.accel_across(accel_across),
+                                          cal_data.accel_through(accel_through));
+
+                       listener.set_acceleration(cal_data.acceleration(accel()));
+                       break;
+               }
+       }
+
+       public AltosEepromRecord next() {
+               int     s = next_start();
+               if (s < 0)
+                       return null;
+               return new AltosEepromRecordMotor(eeprom, s);
+       }
+
+       public AltosEepromRecordMotor(AltosEeprom eeprom, int start) {
+               super(eeprom, start, record_length);
+               log_format = eeprom.config_data().log_format;
+       }
+
+       public AltosEepromRecordMotor(AltosEeprom eeprom) {
+               this(eeprom, 0);
+       }
+}

diff --git a/altoslib/AltosEepromRecordSet.java b/altoslib/AltosEepromRecordSet.java
index 689eaea67553f725aa3fc8b598af8d354309d00b..2a7d97e3002dc93b662452ed255f912f0f3043cf 100644 (file)
--- a/altoslib/AltosEepromRecordSet.java
+++ b/altoslib/AltosEepromRecordSet.java
@@ -106,6 +106,9 @@ public class AltosEepromRecordSet implements AltosRecordSet {
                case AltosLib.AO_LOG_FORMAT_MICROPEAK2:
                        record = new AltosEepromRecordMicroPeak2(eeprom);
                        break;
+               case AltosLib.AO_LOG_FORMAT_EASYMOTOR:
+                       record = new AltosEepromRecordMotor(eeprom);
+                       break;
                }
 
                ordered = new TreeSet<AltosEepromRecord>();

diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java
index 7f8be40328faa7d848965d233727a08c90dcc05b..e66005f2b92f474cd68f2b2ce9442fc09a958074 100644 (file)
--- a/altoslib/AltosIMU.java
+++ b/altoslib/AltosIMU.java
@@ -36,6 +36,7 @@ public class AltosIMU implements Cloneable {
 
        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) {
                switch (imu_type) {
@@ -47,6 +48,8 @@ public class AltosIMU implements Cloneable {
                case  imu_type_telemega_v4:
                case imu_type_easytimer_v1:
                        return counts_per_g_bmx;
+               case imu_type_easymotor_v2:
+                       return counts_per_g_bmx;
                default:
                        return AltosLib.MISSING;
                }
@@ -161,6 +164,8 @@ public class AltosIMU implements Cloneable {
 
        public static final int imu_type_easytimer_v1 = 5;      /* BMX160 */
 
+       public static final int imu_type_easymotor_v2 = 6;      /* ADXL375 (accel only) */
+
        private int accel_across(int imu_type) {
                switch (imu_type) {
                case imu_type_telemega_v1_v2:
@@ -172,6 +177,8 @@ public class AltosIMU implements Cloneable {
                case imu_type_telemega_v4:
                case imu_type_easytimer_v1:
                        return -accel_y;
+               case imu_type_easymotor_v2:
+                       return accel_y;
                default:
                        return AltosLib.MISSING;
                }
@@ -187,6 +194,8 @@ public class AltosIMU implements Cloneable {
                case imu_type_telemega_v4:
                case imu_type_easytimer_v1:
                        return accel_x;
+               case imu_type_easymotor_v2:
+                       return accel_x;
                default:
                        return AltosLib.MISSING;
                }
@@ -308,9 +317,11 @@ public class AltosIMU implements Cloneable {
                        cal_data.set_imu_type(imu_type);
 
                        if (imu != null) {
-                               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)));
+                               if (imu.gyro_x != AltosLib.MISSING) {
+                                       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)));

diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java
index a7f27830cd053a99484a70b793084f5cc6b39507..2d5083cfb157825d5523c1db25266afb574ab39a 100644 (file)
--- a/altoslib/AltosLib.java
+++ b/altoslib/AltosLib.java
@@ -382,6 +382,7 @@ public class AltosLib {
        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_EASYMOTOR = 20;
        public static final int AO_LOG_FORMAT_NONE = 127;
 
        public static boolean isspace(int c) {

diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am
index 0c9392d18a667110fc50b33aee01adfa6907816a..75af5252490281365643d976166c2f722400adb6 100644 (file)
--- a/altoslib/Makefile.am
+++ b/altoslib/Makefile.am
@@ -49,6 +49,7 @@ altoslib_JAVA = \
        AltosEepromRecordGps.java \
        AltosEepromRecordFireTwo.java \
        AltosEepromRecordMicroPeak2.java \
+       AltosEepromRecordMotor.java \
        AltosEepromRecordSet.java \
        AltosEepromChunk.java \
        AltosEepromDownload.java \