altoslib, altosuilib: Get Idle Monitor working with EasyTimer and EasyMotor

[fw/altos] / altoslib / AltosAdxl375.java

diff --git a/altoslib/AltosAdxl375.java b/altoslib/AltosAdxl375.java
index 621cceeb723f1e7f5d4ecc38b46de16635455d99..5e4905412a281665137c9c0813c69229658a717d 100644 (file)
--- a/altoslib/AltosAdxl375.java
+++ b/altoslib/AltosAdxl375.java
@@ -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;
        }