Merge pull request #33 from KalebKE/2.0

2.0
KalebKE · Apr 6, 2020 · 54482a1 · 54482a1
2 parents 0ac19df + 93d0aed
commit 54482a1
Show file tree

Hide file tree

Showing 22 changed files with 556 additions and 305 deletions.
diff --git a/fsensor/build.gradle b/fsensor/build.gradle
@@ -14,8 +14,8 @@ android {
     defaultConfig {
         minSdkVersion 14
         targetSdkVersion 29
-        versionCode 8
-        versionName "1.2.3"
+        versionCode 9
+        versionName "2.0"
 
         testInstrumentationRunner "android.support.test.runner.AndroidJUnitRunner"
 
@@ -38,7 +38,4 @@ dependencies {
     testImplementation 'junit:junit:4.12'
 
     api files('libs/commons-math3-3.6.1.jar')
-
-    api 'io.reactivex.rxjava2:rxandroid:2.1.0'
-    api 'io.reactivex.rxjava2:rxjava:2.2.2'
 }
diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/averaging/MeanFilter.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/averaging/MeanFilter.java
@@ -37,7 +37,7 @@ public class MeanFilter extends AveragingFilter {
 
     private static final String tag = MeanFilter.class.getSimpleName();
 
-    private ArrayDeque<float[]> values;
+    private final ArrayDeque<float[]> values;
     private float[] output;
 
     /**

diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/averaging/MedianFilter.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/averaging/MedianFilter.java
@@ -42,7 +42,7 @@ public class MedianFilter extends AveragingFilter {
     private static final String tag = MedianFilter.class
             .getSimpleName();
 
-    private ArrayDeque<float[]> values;
+    private final ArrayDeque<float[]> values;
     private float[] output;
 
 

diff --git a/...rc/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/OrientationFused.java b/...rc/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/OrientationFused.java
@@ -36,9 +36,9 @@ public abstract class OrientationFused extends BaseFilter {
     // transfer functions (rotations from the gyroscope and
     // acceleration/magnetic, respectively).
     public float timeConstant;
-    protected Quaternion rotationVectorGyroscope;
+    protected Quaternion rotationVector;
     protected long timestamp = 0;
-    protected float[] output;
+    protected float[] output = new float[3];
 
     /**
      * Initialize a singleton instance.
@@ -49,7 +49,7 @@ public OrientationFused() {
 
     public OrientationFused(float timeConstant) {
         this.timeConstant = timeConstant;
-        output = new float[3];
+
     }
 
     @Override
@@ -59,11 +59,11 @@ public float[] getOutput() {
 
     public void reset() {
         timestamp = 0;
-        rotationVectorGyroscope = null;
+        rotationVector = null;
     }
 
     public boolean isBaseOrientationSet() {
-        return rotationVectorGyroscope != null;
+        return rotationVector != null;
     }
 
     /**
@@ -87,6 +87,6 @@ public void setTimeConstant(float timeConstant) {
     public abstract float[] calculateFusedOrientation(float[] gyroscope, long timestamp, float[] acceleration, float[] magnetic);
 
     public void setBaseOrientation(Quaternion baseOrientation) {
-            rotationVectorGyroscope = baseOrientation;
+        rotationVector = baseOrientation;
     }
 }
diff --git a/...ctronics/fsensor/filter/gyroscope/fusion/complementary/OrientationFusedComplementary.java b/...ctronics/fsensor/filter/gyroscope/fusion/complementary/OrientationFusedComplementary.java
@@ -120,23 +120,22 @@ public float[] calculateFusedOrientation(float[] gyroscope, long timestamp, floa
                 Quaternion rotationVectorAccelerationMagnetic = RotationUtil.getOrientationVectorFromAccelerationMagnetic(acceleration, magnetic);
 
                 if(rotationVectorAccelerationMagnetic != null) {
-                    rotationVectorGyroscope = RotationUtil.integrateGyroscopeRotation(rotationVectorGyroscope, gyroscope, dT, EPSILON);
+
+                    rotationVector  = RotationUtil.integrateGyroscopeRotation(rotationVector, gyroscope, dT, EPSILON);
 
                     // Apply the complementary fusedOrientation. // We multiply each rotation by their
                     // coefficients (scalar matrices)...
-                    Quaternion scaledRotationVectorAccelerationMagnetic = rotationVectorAccelerationMagnetic.multiply
-                            (oneMinusAlpha);
+                    Quaternion scaledRotationVectorAccelerationMagnetic = rotationVectorAccelerationMagnetic.multiply(oneMinusAlpha);
 
                     // Scale our quaternion for the gyroscope
-                    Quaternion scaledRotationVectorGyroscope = rotationVectorGyroscope.multiply(alpha);
+                    Quaternion scaledRotationVectorGyroscope = rotationVector.multiply(alpha);
 
-                    // ...and then add the two quaternions together.
+                    //...and then add the two quaternions together.
                     // output[0] = alpha * output[0] + (1 - alpha) * input[0];
-                    rotationVectorGyroscope = scaledRotationVectorGyroscope.add
-                            (scaledRotationVectorAccelerationMagnetic);
-                }
+                    Quaternion result = scaledRotationVectorGyroscope.add(scaledRotationVectorAccelerationMagnetic);
 
-                output = AngleUtils.getAngles(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(), rotationVectorGyroscope.getQ3());
+                    output = AngleUtils.getAngles(result.getQ0(), result.getQ1(), result.getQ2(), result.getQ3());
+                }
             }
 
             this.timestamp = timestamp;

diff --git a/...com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java b/...com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java
@@ -13,6 +13,7 @@
 import org.apache.commons.math3.complex.Quaternion;
 
 import java.util.Arrays;
+import java.util.concurrent.atomic.AtomicBoolean;
 
 /*
  * Copyright 2018, Kircher Electronics, LLC
@@ -60,38 +61,41 @@ public class OrientationFusedKalman extends OrientationFused {
 
     private static final String TAG = OrientationFusedComplementary.class.getSimpleName();
 
-    private RotationKalmanFilter kalmanFilter;
-    private volatile boolean run;
+    private final RotationKalmanFilter kalmanFilter;
+    private final AtomicBoolean run;
     private volatile float dT;
     private volatile float[] output = new float[3];
     private Thread thread;
 
-    private volatile Quaternion rotationOrientation;
+    private volatile Quaternion rotationVectorAccelerationMagnetic;
+    private final double[] vectorGyroscope = new double[4];
+    private final double[] vectorAccelerationMagnetic = new double[4];
 
     public OrientationFusedKalman() {
         this(DEFAULT_TIME_CONSTANT);
     }
 
     public OrientationFusedKalman(float timeConstant) {
         super(timeConstant);
+        run = new AtomicBoolean(false);
         kalmanFilter = new RotationKalmanFilter(new RotationProcessModel(), new RotationMeasurementModel());
     }
 
     public void startFusion() {
-        if (!run && thread == null) {
-            run = true;
+        if (!run.get() && thread == null) {
+            run.set(true);
 
             thread = new Thread(new Runnable() {
                 @Override
                 public void run() {
-                    while (run && !Thread.interrupted()) {
+                    while (run.get() && !Thread.interrupted()) {
 
-                        calculate();
+                        output = calculate();
 
                         try {
                             Thread.sleep(20);
                         } catch (InterruptedException e) {
-                            Log.e(TAG, "Kalman Thread Run", e);
+                            Log.e(TAG, "Kalman Thread", e);
                             Thread.currentThread().interrupt();
                         }
                     }
@@ -105,8 +109,8 @@ public void run() {
     }
 
     public void stopFusion() {
-        if (run && thread != null) {
-            run = false;
+        if (run.get() && thread != null) {
+            run.set(false);
             thread.interrupt();
             thread = null;
         }
@@ -133,21 +137,16 @@ public float[] getOutput() {
      * @return An orientation vector -> @link SensorManager#getOrientation(float[], float[])}
      */
     private float[] calculate() {
-        if (rotationVectorGyroscope != null && rotationOrientation != null && dT != 0) {
+        if (rotationVector != null && rotationVectorAccelerationMagnetic != null && dT != 0) {
+            vectorGyroscope[0] = (float) rotationVector.getVectorPart()[0];
+            vectorGyroscope[1] = (float) rotationVector.getVectorPart()[1];
+            vectorGyroscope[2] = (float) rotationVector.getVectorPart()[2];
+            vectorGyroscope[3] = (float) rotationVector.getScalarPart();
 
-            double[] vectorGyroscope = new double[4];
-
-            vectorGyroscope[0] = (float) rotationVectorGyroscope.getVectorPart()[0];
-            vectorGyroscope[1] = (float) rotationVectorGyroscope.getVectorPart()[1];
-            vectorGyroscope[2] = (float) rotationVectorGyroscope.getVectorPart()[2];
-            vectorGyroscope[3] = (float) rotationVectorGyroscope.getScalarPart();
-
-            double[] vectorAccelerationMagnetic = new double[4];
-
-            vectorAccelerationMagnetic[0] = (float) rotationOrientation.getVectorPart()[0];
-            vectorAccelerationMagnetic[1] = (float) rotationOrientation.getVectorPart()[1];
-            vectorAccelerationMagnetic[2] = (float) rotationOrientation.getVectorPart()[2];
-            vectorAccelerationMagnetic[3] = (float) rotationOrientation.getScalarPart();
+            vectorAccelerationMagnetic[0] = (float) rotationVectorAccelerationMagnetic.getVectorPart()[0];
+            vectorAccelerationMagnetic[1] = (float) rotationVectorAccelerationMagnetic.getVectorPart()[1];
+            vectorAccelerationMagnetic[2] = (float) rotationVectorAccelerationMagnetic.getVectorPart()[2];
+            vectorAccelerationMagnetic[3] = (float) rotationVectorAccelerationMagnetic.getScalarPart();
 
             // Apply the Kalman fusedOrientation... Note that the prediction and correction
             // inputs could be swapped, but the fusedOrientation is much more stable in this
@@ -156,15 +155,12 @@ private float[] calculate() {
             kalmanFilter.correct(vectorAccelerationMagnetic);
 
             // rotation estimation.
-            rotationVectorGyroscope = new Quaternion(kalmanFilter.getStateEstimation()[3],
-                    Arrays.copyOfRange(kalmanFilter.getStateEstimation(), 0, 3));
-
-            output = AngleUtils.getAngles(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(), rotationVectorGyroscope.getQ3());
+            Quaternion result = new Quaternion(kalmanFilter.getStateEstimation()[3], Arrays.copyOfRange(kalmanFilter.getStateEstimation(), 0, 3));
 
-            return output;
+            output = AngleUtils.getAngles(result.getQ0(), result.getQ1(), result.getQ2(), result.getQ3());
         }
 
-        return null;
+        return output;
     }
 
     /**
@@ -177,13 +173,12 @@ private float[] calculate() {
      * @return the fused orientation estimation.
      */
     public float[] calculateFusedOrientation(float[] gyroscope, long timestamp, float[] acceleration, float[] magnetic) {
-
         if(isBaseOrientationSet()) {
             if (this.timestamp != 0) {
                 dT = (timestamp - this.timestamp) * NS2S;
 
-                rotationOrientation = RotationUtil.getOrientationVectorFromAccelerationMagnetic(acceleration, magnetic);
-                rotationVectorGyroscope = RotationUtil.integrateGyroscopeRotation(rotationVectorGyroscope, gyroscope, dT, EPSILON);
+                rotationVectorAccelerationMagnetic = RotationUtil.getOrientationVectorFromAccelerationMagnetic(acceleration, magnetic);
+                rotationVector = RotationUtil.integrateGyroscopeRotation(rotationVector, gyroscope, dT, EPSILON);
             }
             this.timestamp = timestamp;
 

diff --git a/...erelectronics/fsensor/filter/gyroscope/fusion/kalman/filter/RotationMeasurementModel.java b/...erelectronics/fsensor/filter/gyroscope/fusion/kalman/filter/RotationMeasurementModel.java
@@ -21,7 +21,7 @@
  */
 public class RotationMeasurementModel implements MeasurementModel
 {
-	private double noiseCoefficient  = 0.001;
+	private final double noiseCoefficient  = 0.001;
 
 	/**
 	 * The measurement matrix, used to associate the measurement vector to the

diff --git a/...r/src/main/java/com/kircherelectronics/fsensor/linearacceleration/LinearAcceleration.java b/...r/src/main/java/com/kircherelectronics/fsensor/linearacceleration/LinearAcceleration.java
@@ -30,10 +30,9 @@ public abstract class LinearAcceleration {
 
     private static final String tag = LinearAcceleration.class.getSimpleName();
 
-    private float[] output = new float[]
-            {0, 0, 0};
+    private final float[] output = new float[]{0, 0, 0};
 
-    protected BaseFilter filter;
+    protected final BaseFilter filter;
 
     public LinearAcceleration(BaseFilter filter) {
         this.filter = filter;

diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/observer/SensorSubject.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/observer/SensorSubject.java
@@ -0,0 +1,28 @@
+package com.kircherelectronics.fsensor.observer;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class SensorSubject {
+    private final List<SensorObserver> observers = new ArrayList<>();
+
+    public interface SensorObserver {
+        void onSensorChanged(float[] values);
+    }
+
+    public void register(SensorObserver observer) {
+        if(!observers.contains(observer)) {
+            observers.add(observer);
+        }
+    }
+
+    public void unregister(SensorObserver observer) {
+        observers.remove(observer);
+    }
+
+    public void onNext(float[] values) {
+        for(int i = 0; i < observers.size(); i++) {
+            observers.get(i).onSensorChanged(values);
+        }
+    }
+}
diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/FSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/FSensor.java
@@ -1,6 +1,6 @@
 package com.kircherelectronics.fsensor.sensor;
 
-import io.reactivex.subjects.PublishSubject;
+import com.kircherelectronics.fsensor.observer.SensorSubject;
 
 /*
  * Copyright 2018, Kircher Electronics, LLC
@@ -19,5 +19,9 @@
  */
 
 public interface FSensor {
-    PublishSubject<float[]> getPublishSubject();
+    void register(SensorSubject.SensorObserver sensorObserver);
+    void unregister(SensorSubject.SensorObserver sensorObserver);
+
+    void start();
+    void stop();
 }