altoslib, altosuilib: Get Idle Monitor working with EasyTimer and EasyMotor
authorKeith Packard <keithp@keithp.com>
Fri, 23 Oct 2020 23:10:43 +0000 (16:10 -0700)
committerKeith Packard <keithp@keithp.com>
Fri, 23 Oct 2020 23:10:43 +0000 (16:10 -0700)
This involve splitting out the gyro and mag sensor handling from the
3-d accel stuff, displaying only information that is present. The IMU
support now allows for using the along axis as the primary
acceleration indicator. The Adxl375 now allows using all three axes as
the 3d accelerometer.

Signed-off-by: Keith Packard <keithp@keithp.com>
altoslib/AltosAdxl375.java
altoslib/AltosIMU.java
altoslib/AltosIdleFetch.java
altoslib/AltosSensorEasyMotor2.java [new file with mode: 0644]
altoslib/Makefile.am
altosuilib/AltosInfoTable.java

index 621cceeb723f1e7f5d4ecc38b46de16635455d99..5e4905412a281665137c9c0813c69229658a717d 100644 (file)
@@ -22,7 +22,7 @@ import java.util.concurrent.*;
 
 public class AltosAdxl375 implements Cloneable {
 
-       private int     accel;
+       private int[]   accels = new int[3];
        private int     axis;
 
        public static final int X_AXIS = 0;
@@ -35,7 +35,8 @@ public class AltosAdxl375 implements Cloneable {
                        if (axis == AltosLib.MISSING)
                                throw new NumberFormatException("No ADXL375 axis specified");
                        if (items.length >= 3) {
-                               accel = Integer.parseInt(items[2 + axis]);
+                               for (int i = 0; i < 3; i++)
+                                       accels[i] = Integer.parseInt(items[2+i]);
                                return true;
                        }
                }
@@ -45,22 +46,50 @@ public class AltosAdxl375 implements Cloneable {
        public AltosAdxl375 clone() {
                AltosAdxl375    n = new AltosAdxl375(axis);
 
-               n.accel = accel;
+               for (int i = 0; i < 3; i++)
+                       n.accels[i] = accels[i];
                return n;
        }
 
-       static public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException, AltosUnknownProduct {
+       private int accel_along() {
+               return accels[axis];
+       }
+
+       private int accel_across() {
+               if (axis == X_AXIS)
+                       return accels[Y_AXIS];
+               else
+                       return accels[X_AXIS];
+       }
+
+       private int accel_through() {
+               return accels[Z_AXIS];
+       }
+
+       static public void provide_data(AltosDataListener listener, AltosLink link, boolean three_axis, int imu_type) throws InterruptedException, AltosUnknownProduct {
                try {
                        AltosCalData    cal_data = listener.cal_data();
                        AltosAdxl375    adxl375 = new AltosAdxl375(link, cal_data.adxl375_axis);
 
                        if (adxl375 != null) {
-                               int accel = adxl375.accel;
-                               if (cal_data.adxl375_inverted)
+                               int accel = adxl375.accel_along();
+                               if (!cal_data.adxl375_inverted)
                                        accel = -accel;
                                if (cal_data.pad_orientation == 1)
                                        accel = -accel;
                                listener.set_acceleration(cal_data.acceleration(accel));
+                               if (three_axis) {
+                                       cal_data.set_imu_type(imu_type);
+                                       double accel_along = cal_data.accel_along(-accel);
+                                       double accel_across = cal_data.accel_across(adxl375.accel_across());
+                                       double accel_through = cal_data.accel_through(adxl375.accel_through());
+                                       listener.set_accel_ground(accel_along,
+                                                                 accel_across,
+                                                                 accel_through);
+                                       listener.set_accel(accel_along,
+                                                          accel_across,
+                                                          accel_through);
+                               }
                        }
                } catch (TimeoutException te) {
                } catch (NumberFormatException ne) {
@@ -68,7 +97,8 @@ public class AltosAdxl375 implements Cloneable {
        }
 
        public AltosAdxl375() {
-               accel = AltosLib.MISSING;
+               for (int i = 0; i < 3; i++)
+                       accels[i] = AltosLib.MISSING;
                axis = AltosLib.MISSING;
        }
 
index 2944a2a5e1131035b74eb21f2bca7a4cf0420406..f3c85e9aa112945f6e8b0c0cb98d1d2927a3e2ae 100644 (file)
@@ -56,7 +56,10 @@ public class AltosIMU implements Cloneable {
        }
 
        public static double convert_accel(double counts, int imu_type) {
-               return counts / counts_per_g(imu_type) * AltosConvert.gravity;
+               double cpg = counts_per_g(imu_type);
+               if (cpg == AltosLib.MISSING)
+                       return AltosLib.MISSING;
+               return counts / cpg * AltosConvert.gravity;
        }
 
        public static final double      GYRO_FULLSCALE_DEGREES_MPU = 2000.0;
@@ -82,7 +85,10 @@ public class AltosIMU implements Cloneable {
        }
 
        public static double gyro_degrees_per_second(double counts, int imu_type) {
-               return counts / counts_per_degree(imu_type);
+               double cpd = counts_per_degree(imu_type);
+               if (cpd == AltosLib.MISSING)
+                       return AltosLib.MISSING;
+               return counts / cpd;
        }
 
        public static final int imu_axis_x = 0;
@@ -112,7 +118,10 @@ public class AltosIMU implements Cloneable {
        }
 
        public static double convert_gauss(double counts, int imu_type, int imu_axis) {
-               return counts / counts_per_gauss(imu_type, imu_axis);
+               double cpg = counts_per_gauss(imu_type, imu_axis);
+               if (cpg == AltosLib.MISSING)
+                       return AltosLib.MISSING;
+               return counts / cpg;
        }
 
        public boolean parse_string(String line) {
@@ -301,6 +310,15 @@ public class AltosIMU implements Cloneable {
                }
        }
 
+       private static boolean is_primary_accel(int imu_type) {
+               switch (imu_type) {
+               case imu_type_easytimer_v1:
+                       return true;
+               default:
+                       return false;
+               }
+       }
+
        public static int mag_through_axis(int imu_type) {
                return imu_axis_z;
        }
@@ -318,6 +336,7 @@ public class AltosIMU implements Cloneable {
 
                        if (imu != null) {
                                if (imu.gyro_x != 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)),
                                                          cal_data.gyro_yaw(imu.gyro_yaw(imu_type)));
@@ -328,6 +347,14 @@ public class AltosIMU implements Cloneable {
                                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 (is_primary_accel(imu_type)) {
+                                       int accel = imu.accel_along(imu_type);
+                                       if (!cal_data.adxl375_inverted)
+                                               accel = -accel;
+                                       if (cal_data.pad_orientation == 1)
+                                               accel = -accel;
+                                       listener.set_acceleration(cal_data.acceleration(accel));
+                               }
                                if (imu.mag_x != AltosLib.MISSING) {
                                        listener.set_mag(cal_data.mag_along(imu.mag_along(imu_type)),
                                                         cal_data.mag_across(imu.mag_across(imu_type)),
index 1ac075e394c13b01b03658c1a26857dbf053aac4..c4a32788d4617dd319ae190a5fe5667ef510ad7d 100644 (file)
@@ -38,6 +38,7 @@ class AltosIdler {
        static final int        idle_mma655x = 8;
        static final int        idle_ms5607 = 9;
        static final int        idle_adxl375 = 10;
+       static final int        idle_adxl375_easymotor_v2 = 11;
 
        static final int        idle_sensor_tm = 100;
        static final int        idle_sensor_metrum = 101;
@@ -49,6 +50,7 @@ class AltosIdler {
        static final int        idle_sensor_tgps2 = 107;
        static final int        idle_sensor_tmini3 = 108;
        static final int        idle_sensor_easytimer1 = 109;
+       static final int        idle_sensor_easymotor2 = 110;
 
        public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException, TimeoutException, AltosUnknownProduct {
                for (int idler : idlers) {
@@ -81,7 +83,10 @@ class AltosIdler {
                                AltosMma655x.provide_data(listener, link);
                                break;
                        case idle_adxl375:
-                               AltosAdxl375.provide_data(listener, link);
+                               AltosAdxl375.provide_data(listener, link, false, AltosLib.MISSING);
+                               break;
+                       case idle_adxl375_easymotor_v2:
+                               AltosAdxl375.provide_data(listener, link, true, AltosIMU.imu_type_easymotor_v2);
                                break;
                        case idle_ms5607:
                                AltosMs5607.provide_data(listener, link);
@@ -116,6 +121,9 @@ class AltosIdler {
                        case idle_sensor_easytimer1:
                                AltosSensorEasyTimer1.provide_data(listener, link);
                                break;
+                       case idle_sensor_easymotor2:
+                               AltosSensorEasyMotor2.provide_data(listener, link);
+                               break;
                        }
                }
        }
@@ -219,6 +227,9 @@ public class AltosIdleFetch implements AltosDataProvider {
                new AltosIdler("EasyTimer-v1",
                               AltosIdler.idle_imu_et_v1,
                               AltosIdler.idle_sensor_easytimer1),
+               new AltosIdler("EasyMotor-v2",
+                              AltosIdler.idle_adxl375_easymotor_v2,
+                              AltosIdler.idle_sensor_easymotor2),
        };
 
        AltosLink               link;
diff --git a/altoslib/AltosSensorEasyMotor2.java b/altoslib/AltosSensorEasyMotor2.java
new file mode 100644 (file)
index 0000000..b3720a3
--- /dev/null
@@ -0,0 +1,63 @@
+/*
+ * 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.
+ *
+ * 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;
+
+import java.util.concurrent.TimeoutException;
+
+class AltosSensorEasyMotor2 {
+       int             tick;
+       int             v_batt;
+       int             motor_pressure;
+
+       public AltosSensorEasyMotor2(AltosLink link) throws InterruptedException, TimeoutException {
+               String[] items = link.adc();
+               for (int i = 0; i < items.length;) {
+                       if (items[i].equals("tick:")) {
+                               tick = Integer.parseInt(items[i+1]);
+                               i += 2;
+                               continue;
+                       }
+                       if (items[i].equals("motor_pressure:")) {
+                               motor_pressure = Integer.parseInt(items[i+1]);
+                               i += 2;
+                               continue;
+                       }
+                       if (items[i].equals("batt:")) {
+                               v_batt = Integer.parseInt(items[i+1]);
+                               i += 2;
+                               continue;
+                       }
+                       i++;
+               }
+       }
+
+       static public void provide_data(AltosDataListener listener, AltosLink link) throws InterruptedException {
+               try {
+                       AltosSensorEasyMotor2   sensor_easymotor2 = new AltosSensorEasyMotor2(link);
+
+                       listener.set_battery_voltage(AltosConvert.easy_mini_2_voltage(sensor_easymotor2.v_batt));
+                       double pa = AltosConvert.easy_motor_2_motor_pressure(sensor_easymotor2.motor_pressure,
+                                                                            listener.cal_data().ground_motor_pressure);
+                       listener.set_motor_pressure(pa);
+
+               } catch (TimeoutException te) {
+               }
+       }
+}
+
index 75af5252490281365643d976166c2f722400adb6..9c8c66910c4f86c511399fd0497abc93ec7b3613 100644 (file)
@@ -114,6 +114,7 @@ altoslib_JAVA = \
        AltosSensorMetrum.java \
        AltosSensorTGPS1.java \
        AltosSensorTGPS2.java \
+       AltosSensorEasyMotor2.java \
        AltosState.java \
        AltosStateName.java \
        AltosStringInputStream.java \
index 226f112f9542dff7c328b91514704bb4931a9bb8..89d453030ddb3bcfd44f86e050e1577261bffaa2 100644 (file)
@@ -275,16 +275,18 @@ public class AltosInfoTable extends JTable implements AltosFlightDisplay, Hierar
                        info_add_row(3, "Accel along", "%8.1f m/s²", state.accel_along());
                        info_add_row(3, "Accel across", "%8.1f m/s²", state.accel_across());
                        info_add_row(3, "Accel through", "%8.1f m/s²", state.accel_through());
+               }
+               if (state != null && state.gyro_roll() != AltosLib.MISSING) {
                        info_add_row(3, "Gyro roll", "%8.1f °/s", state.gyro_roll());
                        info_add_row(3, "Gyro pitch", "%8.1f °/s", state.gyro_pitch());
                        info_add_row(3, "Gyro yaw", "%8.1f °/s", state.gyro_yaw());
-                       if (state.mag_along() != AltosLib.MISSING) {
-                               /* Report mag in nanoteslas (1 G = 100000 nT (or γ)) */
-                               info_add_row(3, "Mag along", "%8.1f µT", state.mag_along() * 100.0);
-                               info_add_row(3, "Mag across", "%8.1f µT", state.mag_across() * 100.0);
-                               info_add_row(3, "Mag Through", "%8.1f µT", state.mag_through() * 100.0);
-                               info_add_row(3, "Mag Bearing", "%8.1f°", Math.atan2(state.mag_across(), state.mag_through()) * 180/Math.PI);
-                       }
+               }
+               if (state != null && state.mag_along() != AltosLib.MISSING) {
+                       /* Report mag in nanoteslas (1 G = 100000 nT (or γ)) */
+                       info_add_row(3, "Mag along", "%8.1f µT", state.mag_along() * 100.0);
+                       info_add_row(3, "Mag across", "%8.1f µT", state.mag_across() * 100.0);
+                       info_add_row(3, "Mag Through", "%8.1f µT", state.mag_through() * 100.0);
+                       info_add_row(3, "Mag Bearing", "%8.1f°", Math.atan2(state.mag_across(), state.mag_through()) * 180/Math.PI);
                }
 
                if (state != null && state.igniter_voltage != null) {