From 5c953d65b205cf34e21fef31206d08bae230a8c1 Mon Sep 17 00:00:00 2001 From: KalebKE Date: Sun, 5 Apr 2020 15:10:50 -0600 Subject: [PATCH 1/3] v2.0 * For the fused orientations, convert the rotation matrix from the acceleration and magnetic directly into a unit quaternion instead of first converting to Euler angles and then back again. This fixes some bad behavior. * Change the format of the output orientation to match SensorManager.getOrientation(). --- fsensor/build.gradle | 6 +- .../gyroscope/fusion/OrientationFused.java | 12 ++-- .../OrientationFusedComplementary.java | 17 +++--- .../fusion/kalman/OrientationFusedKalman.java | 59 +++++++++---------- .../fsensor/util/angle/AngleUtils.java | 18 +++++- .../fsensor/util/rotation/RotationUtil.java | 37 ++++-------- 6 files changed, 71 insertions(+), 78 deletions(-) diff --git a/fsensor/build.gradle b/fsensor/build.gradle index d2d2c3f..1e92e84 100644 --- 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" @@ -40,5 +40,5 @@ dependencies { 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' + api 'io.reactivex.rxjava2:rxjava:2.2.15' } diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/OrientationFused.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/OrientationFused.java index ba4257d..eca2fc7 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/OrientationFused.java +++ b/fsensor/src/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/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/complementary/OrientationFusedComplementary.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/complementary/OrientationFusedComplementary.java index a76674f..6898e11 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/complementary/OrientationFusedComplementary.java +++ b/fsensor/src/main/java/com/kircherelectronics/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/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java index 7aa7940..826498f 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java +++ b/fsensor/src/main/java/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 @@ -61,12 +62,14 @@ public class OrientationFusedKalman extends OrientationFused { private static final String TAG = OrientationFusedComplementary.class.getSimpleName(); private RotationKalmanFilter kalmanFilter; - private volatile boolean run; + private 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 double[] vectorGyroscope = new double[4]; + private double[] vectorAccelerationMagnetic = new double[4]; public OrientationFusedKalman() { this(DEFAULT_TIME_CONSTANT); @@ -74,24 +77,25 @@ public OrientationFusedKalman() { 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/fsensor/src/main/java/com/kircherelectronics/fsensor/util/angle/AngleUtils.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/util/angle/AngleUtils.java index 48e338e..70653b9 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/util/angle/AngleUtils.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/util/angle/AngleUtils.java @@ -7,6 +7,9 @@ public class AngleUtils { /** * https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm + * https://developer.android.com/reference/android/hardware/SensorManager#getOrientation(float%5B%5D,%20float%5B%5D) + * + * The return format is the same as SensorManager.getOrientation() * * +X to the right * +Y straight up @@ -20,6 +23,17 @@ public class AngleUtils { * Attitude applied second * Bank applied last * + * When it returns, the array values are as follows: + * + * values[0]: Azimuth, angle of rotation about the -z axis. This value represents the angle between the device's y axis and the magnetic north pole. When facing north, this + * angle is 0, when facing south, this angle is π. Likewise, when facing east, this angle is π/2, and when facing west, this angle is -π/2. The range of values is -π to π. + * values[1]: Pitch, angle of rotation about the x axis. This value represents the angle between a plane parallel to the device's screen and a plane parallel to the ground. + * Assuming that the bottom edge of the device faces the user and that the screen is face-up, tilting the top edge of the device toward the ground creates a positive pitch + * angle. The range of values is -π to π. + * values[2]: Roll, angle of rotation about the y axis. This value represents the angle between a plane perpendicular to the device's screen and a plane perpendicular to the + * ground. Assuming that the bottom edge of the device faces the user and that the screen is face-up, tilting the left edge of the device toward the ground creates a + * positive roll angle. The range of values is -π/2 to π/2. + * * @param w * @param z * @param x @@ -49,10 +63,10 @@ public static float[] getAngles(double w, double z, double x, double y) { double sqx = x*x; double sqy = y*y; double sqz = z*z; - heading = Math.atan2(2*y*w-2*x*z , 1 - 2*sqy - 2*sqz); + heading = -Math.atan2(2*y*w-2*x*z , 1 - 2*sqy - 2*sqz); pitch = -Math.asin(2*test); roll = -Math.atan2(2*x*w-2*y*z , 1 - 2*sqx - 2*sqz); - return new float[]{(float)pitch,(float)roll, (float)heading}; + return new float[]{(float) heading, (float) pitch, (float) roll}; } } diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/util/rotation/RotationUtil.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/util/rotation/RotationUtil.java index 7b9da76..de3248b 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/util/rotation/RotationUtil.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/util/rotation/RotationUtil.java @@ -1,11 +1,9 @@ package com.kircherelectronics.fsensor.util.rotation; import android.hardware.SensorManager; +import android.renderscript.Matrix3f; import org.apache.commons.math3.complex.Quaternion; -import org.apache.commons.math3.geometry.euclidean.threed.Rotation; -import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; -import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; import java.util.Arrays; @@ -85,34 +83,21 @@ public static Quaternion integrateGyroscopeRotation(Quaternion previousRotationV public static Quaternion getOrientationVectorFromAccelerationMagnetic(float[] acceleration, float[] magnetic) { float[] rotationMatrix = new float[9]; if (SensorManager.getRotationMatrix(rotationMatrix, null, acceleration, magnetic)) { - float[] rv = new float[3]; - SensorManager.getOrientation(rotationMatrix,rv); - // SensorManager.getOrientation() returns an orientation in Earth frame and that needs to be rotated into device frame so the reported angles - // are indexed with the orientation of the sensors - Rotation rotation = new Rotation(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR, rv[1], -rv[2], rv[0]); - return new Quaternion(rotation.getQ0(), rotation.getQ1(),rotation.getQ2(),rotation.getQ3()); + double[] rotation = getQuaternion(new Matrix3f(rotationMatrix)); + return new Quaternion(rotation[0], rotation[1], rotation[2], rotation[3]); } return null; } - private static double[][] convertTo2DArray(float[] rotation) { - if (rotation.length != 9) { - throw new IllegalStateException("Length must be of 9! Length: " + rotation.length); - } - - double[][] rm = new double[3][3]; + private static double[] getQuaternion(Matrix3f m1) { + double w = Math.sqrt(1.0 + m1.get(0,0) + m1.get(1,1) + m1.get(2,2)) / 2.0; + double w4 = (4.0 * w); + double x = (m1.get(2,1) - m1.get(1,2)) / w4 ; + double y = (m1.get(0,2) - m1.get(2,0)) / w4 ; + double z = (m1.get(1,0) - m1.get(0,1)) / w4 ; - rm[0][0] = rotation[0]; - rm[0][1] = rotation[1]; - rm[0][2] = rotation[2]; - rm[1][0] = rotation[3]; - rm[1][1] = rotation[4]; - rm[1][2] = rotation[5]; - rm[2][0] = rotation[6]; - rm[2][1] = rotation[7]; - rm[2][2] = rotation[8]; - - return rm; + return new double[]{w,x,y,z}; } + } From b052943439ccd879161208d4e97b06be0acd6e98 Mon Sep 17 00:00:00 2001 From: KalebKE Date: Sun, 5 Apr 2020 17:50:18 -0600 Subject: [PATCH 2/3] v2.0 * Remove RxJava as a dependency --- fsensor/build.gradle | 3 - .../fsensor/observer/SensorSubject.java | 28 +++++ .../fsensor/sensor/FSensor.java | 8 +- .../acceleration/AccelerationSensor.java | 48 ++++++--- ...ComplementaryLinearAccelerationSensor.java | 94 +++++++++++----- .../KalmanLinearAccelerationSensor.java | 86 ++++++++++----- .../LinearAccelerationSensor.java | 54 +++++++--- .../LowPassLinearAccelerationSensor.java | 54 +++++++--- .../ComplementaryGyroscopeSensor.java | 95 +++++++++++------ .../sensor/gyroscope/GyroscopeSensor.java | 100 ++++++++++++------ .../gyroscope/KalmanGyroscopeSensor.java | 85 ++++++++++----- 11 files changed, 459 insertions(+), 196 deletions(-) create mode 100644 fsensor/src/main/java/com/kircherelectronics/fsensor/observer/SensorSubject.java diff --git a/fsensor/build.gradle b/fsensor/build.gradle index 1e92e84..b5c70f2 100644 --- a/fsensor/build.gradle +++ b/fsensor/build.gradle @@ -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.15' } diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/observer/SensorSubject.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/observer/SensorSubject.java new file mode 100644 index 0000000..ec55a9b --- /dev/null +++ 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 List 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 index f266d4a..e8e154c 100644 --- 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 getPublishSubject(); + void register(SensorSubject.SensorObserver sensorObserver); + void unregister(SensorSubject.SensorObserver sensorObserver); + + void start(); + void stop(); } diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/AccelerationSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/AccelerationSensor.java index defeb68..24066bf 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/AccelerationSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/AccelerationSensor.java @@ -6,10 +6,9 @@ import android.hardware.SensorEventListener; import android.hardware.SensorManager; +import com.kircherelectronics.fsensor.observer.SensorSubject; import com.kircherelectronics.fsensor.sensor.FSensor; -import io.reactivex.subjects.PublishSubject; - /* * Copyright 2018, Kircher Electronics, LLC * @@ -37,41 +36,56 @@ public class AccelerationSensor implements FSensor { private float[] acceleration = new float[3]; private float[] output = new float[4]; - private int sensorFrequency = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; - private PublishSubject publishSubject; + private SensorSubject sensorSubject; public AccelerationSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); this.listener = new SimpleSensorListener(); - this.publishSubject = PublishSubject.create(); + this.sensorSubject = new SensorSubject(); } @Override - public PublishSubject getPublishSubject() { - return publishSubject; - } - - public void onStart() { + public void start() { startTime = 0; count = 0; - registerSensors(sensorFrequency); + registerSensors(sensorDelay); } - public void onStop() { + @Override + public void stop() { unregisterSensors(); } - public void setSensorFrequency(int sensorFrequency) { - this.sensorFrequency = sensorFrequency; + @Override + public void register(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.register(sensorObserver); + } + + @Override + public void unregister(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.unregister(sensorObserver); + } + + /** + * Set the sensor frequency. + * @param sensorDelay Must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or SensorManager.SENSOR_DELAY_UI + */ + public void setSensorDelay(int sensorDelay) { + if(sensorDelay != SensorManager.SENSOR_DELAY_FASTEST && sensorDelay != SensorManager.SENSOR_DELAY_GAME && sensorDelay != SensorManager.SENSOR_DELAY_NORMAL && sensorDelay != SensorManager.SENSOR_DELAY_UI) { + throw new IllegalStateException("Sensor Frequency must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or " + + "SensorManager.SENSOR_DELAY_UI"); + } + this.sensorDelay = sensorDelay; } public void reset() { - onStop(); + stop(); acceleration = new float[3]; output = new float[4]; - onStart(); + start(); } private float calculateSensorFrequency() { @@ -109,7 +123,7 @@ private void unregisterSensors() { private void setOutput(float[] value) { System.arraycopy(value, 0, output, 0, value.length); output[3] = calculateSensorFrequency(); - publishSubject.onNext(output); + sensorSubject.onNext(output); } private class SimpleSensorListener implements SensorEventListener { diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/ComplementaryLinearAccelerationSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/ComplementaryLinearAccelerationSensor.java index c4b62b5..bf89c35 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/ComplementaryLinearAccelerationSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/ComplementaryLinearAccelerationSensor.java @@ -9,11 +9,10 @@ import com.kircherelectronics.fsensor.filter.gyroscope.fusion.complementary.OrientationFusedComplementary; import com.kircherelectronics.fsensor.linearacceleration.LinearAcceleration; import com.kircherelectronics.fsensor.linearacceleration.LinearAccelerationFusion; +import com.kircherelectronics.fsensor.observer.SensorSubject; import com.kircherelectronics.fsensor.sensor.FSensor; import com.kircherelectronics.fsensor.util.rotation.RotationUtil; -import io.reactivex.subjects.PublishSubject; - /* * Copyright 2018, Kircher Electronics, LLC * @@ -38,9 +37,6 @@ public class ComplementaryLinearAccelerationSensor implements FSensor { private float startTime = 0; private int count = 0; - private boolean hasRotation = false; - private boolean hasMagnetic = false; - private float[] magnetic = new float[3]; private float[] rawAcceleration = new float[3]; private float[] rotation = new float[3]; @@ -50,49 +46,91 @@ public class ComplementaryLinearAccelerationSensor implements FSensor { private LinearAcceleration linearAccelerationFilterComplimentary; private OrientationFusedComplementary orientationFusionComplimentary; - private int sensorFrequency = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorType = Sensor.TYPE_GYROSCOPE; - private PublishSubject publishSubject; + private SensorSubject sensorSubject; public ComplementaryLinearAccelerationSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); this.listener = new SimpleSensorListener(); - this.publishSubject = PublishSubject.create(); + this.sensorSubject = new SensorSubject(); initializeFSensorFusions(); } + /** + * Start the sensor. + */ @Override - public PublishSubject getPublishSubject() { - return publishSubject; - } - - public void onStart() { + public void start() { startTime = 0; count = 0; - registerSensors(sensorFrequency); + registerSensors(sensorDelay); } - public void onStop() { + /** + * Stop the sensor. + */ + @Override + public void stop() { unregisterSensors(); } - public void setSensorFrequency(int sensorFrequency) { - this.sensorFrequency = sensorFrequency; + @Override + public void register(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.register(sensorObserver); + } + + @Override + public void unregister(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.unregister(sensorObserver); + } + + /** + * Set the gyroscope sensor type. + * @param sensorType must be Sensor.TYPE_GYROSCOPE or Sensor.TYPE_GYROSCOPE_UNCALIBRATED + */ + public void setSensorType(int sensorType) { + if(sensorType != Sensor.TYPE_GYROSCOPE && sensorType != Sensor.TYPE_GYROSCOPE_UNCALIBRATED) { + throw new IllegalStateException("Sensor Type must be Sensor.TYPE_GYROSCOPE or Sensor.TYPE_GYROSCOPE_UNCALIBRATED"); + } + + this.sensorType = sensorType; + + } + + /** + * Set the sensor frequency. + * @param sensorDelay Must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or SensorManager.SENSOR_DELAY_UI + */ + public void setSensorDelay(int sensorDelay) { + if(sensorDelay != SensorManager.SENSOR_DELAY_FASTEST && sensorDelay != SensorManager.SENSOR_DELAY_GAME && sensorDelay != SensorManager.SENSOR_DELAY_NORMAL && sensorDelay != SensorManager.SENSOR_DELAY_UI) { + throw new IllegalStateException("Sensor Frequency must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or " + + "SensorManager.SENSOR_DELAY_UI"); + } + this.sensorDelay = sensorDelay; } + /** + * The filter coefficient. + * + * @param timeConstant A floating point value between 0 and 1. Exclusive of 0, inclusive of 1. + */ public void setFSensorComplimentaryLinearAccelerationTimeConstant(float timeConstant) { orientationFusionComplimentary.setTimeConstant(timeConstant); } + /** + * Reset the sensor. + */ public void reset() { - onStop(); + stop(); magnetic = new float[3]; acceleration = new float[3]; rotation = new float[3]; output = new float[4]; - hasRotation = false; - hasMagnetic = false; - onStart(); + listener.reset(); + start(); } private float calculateSensorFrequency() { @@ -151,7 +189,7 @@ private void registerSensors(int sensorDelay) { // Register for sensor updates. sensorManager.registerListener(listener, - sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE_UNCALIBRATED), + sensorManager.getDefaultSensor(sensorType), sensorDelay); } @@ -163,11 +201,19 @@ private void unregisterSensors() { private void setOutput(float[] value) { System.arraycopy(value, 0, output, 0, value.length); output[3] = calculateSensorFrequency(); - publishSubject.onNext(output); + sensorSubject.onNext(output); } private class SimpleSensorListener implements SensorEventListener { + private boolean hasRotation = false; + private boolean hasMagnetic = false; + + private void reset() { + hasRotation = false; + hasMagnetic = false; + } + @Override public void onSensorChanged(SensorEvent event) { if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) { @@ -185,7 +231,7 @@ public void onSensorChanged(SensorEvent event) { } else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) { processMagnetic(event.values); hasMagnetic = true; - } else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE_UNCALIBRATED) { + } else if (event.sensor.getType() == sensorType) { processRotation(event.values); hasRotation = true; } diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/KalmanLinearAccelerationSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/KalmanLinearAccelerationSensor.java index 535558a..18b29a2 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/KalmanLinearAccelerationSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/KalmanLinearAccelerationSensor.java @@ -9,11 +9,10 @@ import com.kircherelectronics.fsensor.filter.gyroscope.fusion.kalman.OrientationFusedKalman; import com.kircherelectronics.fsensor.linearacceleration.LinearAcceleration; import com.kircherelectronics.fsensor.linearacceleration.LinearAccelerationFusion; +import com.kircherelectronics.fsensor.observer.SensorSubject; import com.kircherelectronics.fsensor.sensor.FSensor; import com.kircherelectronics.fsensor.util.rotation.RotationUtil; -import io.reactivex.subjects.PublishSubject; - /* * Copyright 2018, Kircher Electronics, LLC * @@ -38,9 +37,6 @@ public class KalmanLinearAccelerationSensor implements FSensor { private float startTime = 0; private int count = 0; - private boolean hasRotation = false; - private boolean hasMagnetic = false; - private float[] magnetic = new float[3]; private float[] rawAcceleration = new float[3]; private float[] rotation = new float[3]; @@ -50,47 +46,81 @@ public class KalmanLinearAccelerationSensor implements FSensor { private LinearAcceleration linearAccelerationFilterKalman; private OrientationFusedKalman orientationFusionKalman; - private int sensorFrequency = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorType = Sensor.TYPE_GYROSCOPE; - private PublishSubject publishSubject; + private SensorSubject sensorSubject; public KalmanLinearAccelerationSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); this.listener = new SimpleSensorListener(); - this.publishSubject = PublishSubject.create(); + this.sensorSubject = new SensorSubject(); initializeFSensorFusions(); } - @Override - public PublishSubject getPublishSubject() { - return publishSubject; - } - public void onStart() { + /** + * Start the sensor. + */ + @Override + public void start() { startTime = 0; count = 0; - registerSensors(sensorFrequency); + registerSensors(sensorDelay); orientationFusionKalman.startFusion(); } - public void onStop() { + /** + * Stop the sensor. + */ + @Override + public void stop() { orientationFusionKalman.stopFusion(); unregisterSensors(); } - public void setSensorFrequency(int sensorFrequency) { - this.sensorFrequency = sensorFrequency; + @Override + public void register(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.register(sensorObserver); + } + + @Override + public void unregister(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.unregister(sensorObserver); + } + + /** + * Set the gyroscope sensor type. + * @param sensorType must be Sensor.TYPE_GYROSCOPE or Sensor.TYPE_GYROSCOPE_UNCALIBRATED + */ + public void setSensorType(int sensorType) { + if(sensorType != Sensor.TYPE_GYROSCOPE && sensorType != Sensor.TYPE_GYROSCOPE_UNCALIBRATED) { + throw new IllegalStateException("Sensor Type must be Sensor.TYPE_GYROSCOPE or Sensor.TYPE_GYROSCOPE_UNCALIBRATED"); + } + + this.sensorType = sensorType; + } + + /** + * Set the sensor frequency. + * @param sensorDelay Must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or SensorManager.SENSOR_DELAY_UI + */ + public void setSensorDelay(int sensorDelay) { + if(sensorDelay != SensorManager.SENSOR_DELAY_FASTEST && sensorDelay != SensorManager.SENSOR_DELAY_GAME && sensorDelay != SensorManager.SENSOR_DELAY_NORMAL && sensorDelay != SensorManager.SENSOR_DELAY_UI) { + throw new IllegalStateException("Sensor Frequency must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or " + + "SensorManager.SENSOR_DELAY_UI"); + } + this.sensorDelay = sensorDelay; } public void reset() { - onStop(); + stop(); magnetic = new float[3]; acceleration = new float[3]; rotation = new float[3]; output = new float[4]; - hasRotation = false; - hasMagnetic = false; - onStart(); + listener.reset(); + start(); } private float calculateSensorFrequency() { @@ -147,7 +177,7 @@ private void registerSensors(int sensorDelay) { // Register for sensor updates. sensorManager.registerListener(listener, - sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE_UNCALIBRATED), + sensorManager.getDefaultSensor(sensorType), sensorDelay); } @@ -159,11 +189,19 @@ private void unregisterSensors() { private void setOutput(float[] value) { System.arraycopy(value, 0, output, 0, value.length); output[3] = calculateSensorFrequency(); - publishSubject.onNext(output); + sensorSubject.onNext(output); } private class SimpleSensorListener implements SensorEventListener { + private boolean hasRotation = false; + private boolean hasMagnetic = false; + + private void reset() { + hasRotation = false; + hasMagnetic = false; + } + @Override public void onSensorChanged(SensorEvent event) { if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) { @@ -181,7 +219,7 @@ public void onSensorChanged(SensorEvent event) { } else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) { processMagnetic(event.values); hasMagnetic = true; - } else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE_UNCALIBRATED) { + } else if (event.sensor.getType() == sensorType) { processRotation(event.values); hasRotation = true; } diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LinearAccelerationSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LinearAccelerationSensor.java index 99b9a2c..1a3794f 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LinearAccelerationSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LinearAccelerationSensor.java @@ -6,10 +6,9 @@ import android.hardware.SensorEventListener; import android.hardware.SensorManager; +import com.kircherelectronics.fsensor.observer.SensorSubject; import com.kircherelectronics.fsensor.sensor.FSensor; -import io.reactivex.subjects.PublishSubject; - /* * Copyright 2018, Kircher Electronics, LLC * @@ -37,41 +36,62 @@ public class LinearAccelerationSensor implements FSensor { private float[] acceleration = new float[3]; private float[] output = new float[4]; - private int sensorFrequency = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; - private PublishSubject publishSubject; + private SensorSubject sensorSubject; public LinearAccelerationSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); this.listener = new SimpleSensorListener(); - this.publishSubject = PublishSubject.create(); + this.sensorSubject = new SensorSubject(); } + /** + * Start the sensor. + */ @Override - public PublishSubject getPublishSubject() { - return publishSubject; - } - - public void onStart() { + public void start() { startTime = 0; count = 0; - registerSensors(sensorFrequency); + registerSensors(sensorDelay); } - public void onStop() { + /** + * Stop the sensor. + */ + @Override + public void stop() { unregisterSensors(); } - public void setSensorFrequency(int sensorFrequency) { - this.sensorFrequency = sensorFrequency; + @Override + public void register(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.register(sensorObserver); + } + + @Override + public void unregister(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.unregister(sensorObserver); + } + + /** + * Set the sensor frequency. + * @param sensorDelay Must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or SensorManager.SENSOR_DELAY_UI + */ + public void setSensorDelay(int sensorDelay) { + if(sensorDelay != SensorManager.SENSOR_DELAY_FASTEST && sensorDelay != SensorManager.SENSOR_DELAY_GAME && sensorDelay != SensorManager.SENSOR_DELAY_NORMAL && sensorDelay != SensorManager.SENSOR_DELAY_UI) { + throw new IllegalStateException("Sensor Frequency must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or " + + "SensorManager.SENSOR_DELAY_UI"); + } + this.sensorDelay = sensorDelay; } public void reset() { - onStop(); + stop(); acceleration = new float[3]; output = new float[4]; - onStart(); + start(); } private float calculateSensorFrequency() { @@ -109,7 +129,7 @@ private void unregisterSensors() { private void setOutput(float[] value) { System.arraycopy(value, 0, output, 0, value.length); output[3] = calculateSensorFrequency(); - publishSubject.onNext(output); + sensorSubject.onNext(output); } private class SimpleSensorListener implements SensorEventListener { diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LowPassLinearAccelerationSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LowPassLinearAccelerationSensor.java index a0e4920..554a4a4 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LowPassLinearAccelerationSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LowPassLinearAccelerationSensor.java @@ -9,10 +9,9 @@ import com.kircherelectronics.fsensor.filter.averaging.LowPassFilter; import com.kircherelectronics.fsensor.linearacceleration.LinearAcceleration; import com.kircherelectronics.fsensor.linearacceleration.LinearAccelerationAveraging; +import com.kircherelectronics.fsensor.observer.SensorSubject; import com.kircherelectronics.fsensor.sensor.FSensor; -import io.reactivex.subjects.PublishSubject; - /* * Copyright 2018, Kircher Electronics, LLC * @@ -45,35 +44,56 @@ public class LowPassLinearAccelerationSensor implements FSensor { private LowPassFilter lpfGravity; - private int sensorFrequency = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; - private PublishSubject publishSubject; + private SensorSubject sensorSubject; public LowPassLinearAccelerationSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); this.listener = new SimpleSensorListener(); - this.publishSubject = PublishSubject.create(); + this.sensorSubject = new SensorSubject(); initializeFSensorFusions(); } + /** + * Stop the sensor. + */ @Override - public PublishSubject getPublishSubject() { - return publishSubject; - } - - public void onStart() { + public void start() { startTime = 0; count = 0; - registerSensors(sensorFrequency); + registerSensors(sensorDelay); } - public void onStop() { + /** + * Stop the sensor. + */ + @Override + public void stop() { unregisterSensors(); } - public void setSensorFrequency(int sensorFrequency) { - this.sensorFrequency = sensorFrequency; + @Override + public void register(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.register(sensorObserver); + } + + @Override + public void unregister(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.unregister(sensorObserver); + } + + /** + * Set the sensor frequency. + * @param sensorDelay Must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or SensorManager.SENSOR_DELAY_UI + */ + public void setSensorDelay(int sensorDelay) { + if(sensorDelay != SensorManager.SENSOR_DELAY_FASTEST && sensorDelay != SensorManager.SENSOR_DELAY_GAME && sensorDelay != SensorManager.SENSOR_DELAY_NORMAL && sensorDelay != SensorManager.SENSOR_DELAY_UI) { + throw new IllegalStateException("Sensor Frequency must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or " + + "SensorManager.SENSOR_DELAY_UI"); + } + this.sensorDelay = sensorDelay; } public void setFSensorLpfLinearAccelerationTimeConstant(float timeConstant) { @@ -81,10 +101,10 @@ public void setFSensorLpfLinearAccelerationTimeConstant(float timeConstant) { } public void reset() { - onStop(); + stop(); acceleration = new float[3]; output = new float[4]; - onStart(); + start(); } private float calculateSensorFrequency() { @@ -143,7 +163,7 @@ private void unregisterSensors() { private void setOutput(float[] value) { System.arraycopy(value, 0, output, 0, value.length); output[3] = calculateSensorFrequency(); - publishSubject.onNext(output); + sensorSubject.onNext(output); } private class SimpleSensorListener implements SensorEventListener { diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/ComplementaryGyroscopeSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/ComplementaryGyroscopeSensor.java index 651c7c9..bb6bcad 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/ComplementaryGyroscopeSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/ComplementaryGyroscopeSensor.java @@ -7,11 +7,10 @@ import android.hardware.SensorManager; import com.kircherelectronics.fsensor.filter.gyroscope.fusion.complementary.OrientationFusedComplementary; +import com.kircherelectronics.fsensor.observer.SensorSubject; import com.kircherelectronics.fsensor.sensor.FSensor; import com.kircherelectronics.fsensor.util.rotation.RotationUtil; -import io.reactivex.subjects.PublishSubject; - /* * Copyright 2018, Kircher Electronics, LLC * @@ -36,9 +35,6 @@ public class ComplementaryGyroscopeSensor implements FSensor { private float startTime = 0; private int count = 0; - private boolean hasAcceleration = false; - private boolean hasMagnetic = false; - private float[] magnetic = new float[3]; private float[] acceleration = new float[3]; private float[] rotation = new float[3]; @@ -46,34 +42,68 @@ public class ComplementaryGyroscopeSensor implements FSensor { private OrientationFusedComplementary orientationFusionComplimentary; - private int sensorFrequency = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorType = Sensor.TYPE_GYROSCOPE; - private PublishSubject publishSubject; + private SensorSubject sensorSubject; public ComplementaryGyroscopeSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); this.listener = new SimpleSensorListener(); - this.publishSubject = PublishSubject.create(); + this.sensorSubject = new SensorSubject(); initializeFSensorFusions(); } + /** + * Start the sensor. + */ @Override - public PublishSubject getPublishSubject() { - return publishSubject; - } - - public void onStart() { + public void start() { startTime = 0; count = 0; - registerSensors(sensorFrequency); + registerSensors(sensorDelay); } - public void onStop() { + /** + * Stop the sensor. + */ + @Override + public void stop() { unregisterSensors(); } - public void setSensorFrequency(int sensorFrequency) { - this.sensorFrequency = sensorFrequency; + @Override + public void register(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.register(sensorObserver); + } + + @Override + public void unregister(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.unregister(sensorObserver); + } + + /** + * Set the gyroscope sensor type. + * @param sensorType must be Sensor.TYPE_GYROSCOPE or Sensor.TYPE_GYROSCOPE_UNCALIBRATED + */ + public void setSensorType(int sensorType) { + if(sensorType != Sensor.TYPE_GYROSCOPE && sensorType != Sensor.TYPE_GYROSCOPE_UNCALIBRATED) { + throw new IllegalStateException("Sensor Type must be Sensor.TYPE_GYROSCOPE or Sensor.TYPE_GYROSCOPE_UNCALIBRATED"); + } + + this.sensorType = sensorType; + } + + /** + * Set the sensor frequency. + * @param sensorDelay Must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or SensorManager.SENSOR_DELAY_UI + */ + public void setSensorDelay(int sensorDelay) { + if(sensorDelay != SensorManager.SENSOR_DELAY_FASTEST && sensorDelay != SensorManager.SENSOR_DELAY_GAME && sensorDelay != SensorManager.SENSOR_DELAY_NORMAL && sensorDelay != SensorManager.SENSOR_DELAY_UI) { + throw new IllegalStateException("Sensor Frequency must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or " + + "SensorManager.SENSOR_DELAY_UI"); + } + this.sensorDelay = sensorDelay; } public void setFSensorComplimentaryTimeConstant(float timeConstant) { @@ -81,14 +111,13 @@ public void setFSensorComplimentaryTimeConstant(float timeConstant) { } public void reset() { - onStop(); + stop(); magnetic = new float[3]; acceleration = new float[3]; rotation = new float[3]; output = new float[4]; - hasAcceleration = false; - hasMagnetic = false; - onStart(); + listener.reset(); + start(); } private float calculateSensorFrequency() { @@ -140,7 +169,7 @@ private void registerSensors(int sensorDelay) { // Register for sensor updates. sensorManager.registerListener(listener, - sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE_UNCALIBRATED), + sensorManager.getDefaultSensor(sensorType), sensorDelay); } @@ -152,28 +181,28 @@ private void unregisterSensors() { private void setValue(float[] value) { System.arraycopy(value, 0, output, 0, value.length); output[3] = calculateSensorFrequency(); - publishSubject.onNext(output); + sensorSubject.onNext(output); } private class SimpleSensorListener implements SensorEventListener { - private int sensorEventThreshold = 100; - private int numAccelerationEvents = 0; - private int numMagneticEvents = 0; + private boolean hasAcceleration = false; + private boolean hasMagnetic = false; + + private void reset() { + hasAcceleration = false; + hasMagnetic = false; + } @Override public void onSensorChanged(SensorEvent event) { if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) { processAcceleration(event.values); - if(numAccelerationEvents++ > sensorEventThreshold) { - hasAcceleration = true; - } + hasAcceleration = true; } else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) { processMagnetic(event.values); - if(numMagneticEvents ++ > sensorEventThreshold) { - hasMagnetic = true; - } - } else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE_UNCALIBRATED) { + hasMagnetic = true; + } else if (event.sensor.getType() == sensorType) { processRotation(event.values); if (!orientationFusionComplimentary.isBaseOrientationSet()) { if (hasAcceleration && hasMagnetic) { diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/GyroscopeSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/GyroscopeSensor.java index 67f526c..4c466fd 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/GyroscopeSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/GyroscopeSensor.java @@ -7,13 +7,10 @@ import android.hardware.SensorManager; import com.kircherelectronics.fsensor.filter.gyroscope.OrientationGyroscope; +import com.kircherelectronics.fsensor.observer.SensorSubject; import com.kircherelectronics.fsensor.sensor.FSensor; import com.kircherelectronics.fsensor.util.rotation.RotationUtil; -import org.apache.commons.math3.complex.Quaternion; - -import io.reactivex.subjects.PublishSubject; - /* * Copyright 2018, Kircher Electronics, LLC * @@ -39,9 +36,6 @@ public class GyroscopeSensor implements FSensor { private float startTime = 0; private int count = 0; - private boolean hasAcceleration = false; - private boolean hasMagnetic = false; - private float[] magnetic = new float[3]; private float[] acceleration = new float[3]; private float[] rotation = new float[3]; @@ -49,45 +43,81 @@ public class GyroscopeSensor implements FSensor { private OrientationGyroscope orientationGyroscope; - private int sensorFrequency = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorType = Sensor.TYPE_GYROSCOPE; - private PublishSubject publishSubject; + private SensorSubject sensorSubject; public GyroscopeSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); this.listener = new SimpleSensorListener(); - this.publishSubject = PublishSubject.create(); + this.sensorSubject = new SensorSubject(); initializeFSensorFusions(); } + /** + * Start the sensor. + */ @Override - public PublishSubject getPublishSubject() { - return publishSubject; - } - - public void onStart() { + public void start() { startTime = 0; count = 0; - registerSensors(sensorFrequency); + registerSensors(sensorDelay); } - public void onStop() { + /** + * Stop the sensor. + */ + @Override + public void stop() { unregisterSensors(); } - public void setSensorFrequency(int sensorFrequency) { - this.sensorFrequency = sensorFrequency; + @Override + public void register(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.register(sensorObserver); } + @Override + public void unregister(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.unregister(sensorObserver); + } + + /** + * Set the gyroscope sensor type. + * @param sensorType must be Sensor.TYPE_GYROSCOPE or Sensor.TYPE_GYROSCOPE_UNCALIBRATED + */ + public void setSensorType(int sensorType) { + if(sensorType != Sensor.TYPE_GYROSCOPE && sensorType != Sensor.TYPE_GYROSCOPE_UNCALIBRATED) { + throw new IllegalStateException("Sensor Type must be Sensor.TYPE_GYROSCOPE or Sensor.TYPE_GYROSCOPE_UNCALIBRATED"); + } + + this.sensorType = sensorType; + } + + /** + * Set the sensor frequency. + * @param sensorDelay Must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or SensorManager.SENSOR_DELAY_UI + */ + public void setSensorDelay(int sensorDelay) { + if(sensorDelay != SensorManager.SENSOR_DELAY_FASTEST && sensorDelay != SensorManager.SENSOR_DELAY_GAME && sensorDelay != SensorManager.SENSOR_DELAY_NORMAL && sensorDelay != SensorManager.SENSOR_DELAY_UI) { + throw new IllegalStateException("Sensor Frequency must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or " + + "SensorManager.SENSOR_DELAY_UI"); + } + this.sensorDelay = sensorDelay; + } + + /** + * Reset the sensor. + */ public void reset() { - onStop(); + stop(); magnetic = new float[3]; acceleration = new float[3]; rotation = new float[3]; output = new float[4]; - hasAcceleration = false; - hasMagnetic = false; - onStart(); + listener.reset(); + start(); } private float calculateSensorFrequency() { @@ -139,7 +169,7 @@ private void registerSensors(int sensorDelay) { // Register for sensor updates. sensorManager.registerListener(listener, - sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE), + sensorManager.getDefaultSensor(sensorType), sensorDelay); } @@ -151,28 +181,28 @@ private void unregisterSensors() { private void setOutput(float[] value) { System.arraycopy(value, 0, output, 0, value.length); output[3] = calculateSensorFrequency(); - publishSubject.onNext(output); + sensorSubject.onNext(output); } private class SimpleSensorListener implements SensorEventListener { - private int sensorEventThreshold = 500; - private int numAccelerationEvents = 0; - private int numMagneticEvents = 0; + private boolean hasAcceleration = false; + private boolean hasMagnetic = false; + + private void reset() { + hasAcceleration = false; + hasMagnetic = false; + } @Override public void onSensorChanged(SensorEvent event) { if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) { processAcceleration(event.values); - if(numAccelerationEvents++ > sensorEventThreshold) { - hasAcceleration = true; - } + hasAcceleration = true; } else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) { processMagnetic(event.values); - if(numMagneticEvents ++ > sensorEventThreshold) { - hasMagnetic = true; - } - } else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) { + hasMagnetic = true; + } else if (event.sensor.getType() == sensorType) { processRotation(event.values); if (!orientationGyroscope.isBaseOrientationSet()) { diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/KalmanGyroscopeSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/KalmanGyroscopeSensor.java index d6e083b..cd3952f 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/KalmanGyroscopeSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/KalmanGyroscopeSensor.java @@ -7,11 +7,10 @@ import android.hardware.SensorManager; import com.kircherelectronics.fsensor.filter.gyroscope.fusion.kalman.OrientationFusedKalman; +import com.kircherelectronics.fsensor.observer.SensorSubject; import com.kircherelectronics.fsensor.sensor.FSensor; import com.kircherelectronics.fsensor.util.rotation.RotationUtil; -import io.reactivex.subjects.PublishSubject; - /* * Copyright 2018, Kircher Electronics, LLC * @@ -36,9 +35,6 @@ public class KalmanGyroscopeSensor implements FSensor { private float startTime = 0; private int count = 0; - private boolean hasAcceleration = false; - private boolean hasMagnetic = false; - private float[] magnetic = new float[3]; private float[] acceleration = new float[3]; private float[] rotation = new float[3]; @@ -46,47 +42,80 @@ public class KalmanGyroscopeSensor implements FSensor { private OrientationFusedKalman orientationFusionKalman; - private int sensorFrequency = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; + private int sensorType = Sensor.TYPE_GYROSCOPE; - private PublishSubject publishSubject; + private SensorSubject sensorSubject; public KalmanGyroscopeSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); this.listener = new SimpleSensorListener(); - this.publishSubject = PublishSubject.create(); + this.sensorSubject = new SensorSubject(); initializeFSensorFusions(); } + /** + * Start the sensor. + */ @Override - public PublishSubject getPublishSubject() { - return publishSubject; - } - - public void onStart() { + public void start() { startTime = 0; count = 0; - registerSensors(sensorFrequency); + registerSensors(sensorDelay); orientationFusionKalman.startFusion(); } - public void onStop() { + /** + * Stop the sensor. + */ + @Override + public void stop() { orientationFusionKalman.stopFusion(); unregisterSensors(); } - public void setSensorFrequency(int sensorFrequency) { - this.sensorFrequency = sensorFrequency; + @Override + public void register(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.register(sensorObserver); + } + + @Override + public void unregister(SensorSubject.SensorObserver sensorObserver) { + sensorSubject.unregister(sensorObserver); + } + + /** + * Set the gyroscope sensor type. + * @param sensorType must be Sensor.TYPE_GYROSCOPE or Sensor.TYPE_GYROSCOPE_UNCALIBRATED + */ + public void setSensorType(int sensorType) { + if(sensorType != Sensor.TYPE_GYROSCOPE && sensorType != Sensor.TYPE_GYROSCOPE_UNCALIBRATED) { + throw new IllegalStateException("Sensor Type must be Sensor.TYPE_GYROSCOPE or Sensor.TYPE_GYROSCOPE_UNCALIBRATED"); + } + + this.sensorType = sensorType; + } + + /** + * Set the sensor frequency. + * @param sensorDelay Must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or SensorManager.SENSOR_DELAY_UI + */ + public void setSensorDelay(int sensorDelay) { + if(sensorDelay != SensorManager.SENSOR_DELAY_FASTEST && sensorDelay != SensorManager.SENSOR_DELAY_GAME && sensorDelay != SensorManager.SENSOR_DELAY_NORMAL && sensorDelay != SensorManager.SENSOR_DELAY_UI) { + throw new IllegalStateException("Sensor Frequency must be SensorManager.SENSOR_DELAY_FASTEST, SensorManager.SENSOR_DELAY_GAME, SensorManager.SENSOR_DELAY_NORMAL or " + + "SensorManager.SENSOR_DELAY_UI"); + } + this.sensorDelay = sensorDelay; } public void reset() { - onStop(); + stop(); magnetic = new float[3]; acceleration = new float[3]; rotation = new float[3]; output = new float[4]; - hasAcceleration = false; - hasMagnetic = false; - onStart(); + listener.reset(); + start(); } private float calculateSensorFrequency() { @@ -138,7 +167,7 @@ private void registerSensors(int sensorDelay) { // Register for sensor updates. sensorManager.registerListener(listener, - sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE_UNCALIBRATED), + sensorManager.getDefaultSensor(sensorType), sensorDelay); } @@ -150,11 +179,19 @@ private void unregisterSensors() { private void setOutput(float[] value) { System.arraycopy(value, 0, output, 0, value.length); output[3] = calculateSensorFrequency(); - publishSubject.onNext(output); + sensorSubject.onNext(output); } private class SimpleSensorListener implements SensorEventListener { + private boolean hasAcceleration = false; + private boolean hasMagnetic = false; + + private void reset() { + hasAcceleration = false; + hasMagnetic = false; + } + @Override public void onSensorChanged(SensorEvent event) { if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) { @@ -163,7 +200,7 @@ public void onSensorChanged(SensorEvent event) { } else if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) { processMagnetic(event.values); hasMagnetic = true; - } else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE_UNCALIBRATED) { + } else if (event.sensor.getType() == sensorType) { processRotation(event.values); if (!orientationFusionKalman.isBaseOrientationSet()) { From 93d0aed45ee352025775c69a957bc766dbda3630 Mon Sep 17 00:00:00 2001 From: KalebKE Date: Sun, 5 Apr 2020 18:00:26 -0600 Subject: [PATCH 3/3] v2.0 * Code clean up. --- .../fsensor/filter/averaging/MeanFilter.java | 2 +- .../fsensor/filter/averaging/MedianFilter.java | 2 +- .../gyroscope/fusion/kalman/OrientationFusedKalman.java | 8 ++++---- .../fusion/kalman/filter/RotationMeasurementModel.java | 2 +- .../fsensor/linearacceleration/LinearAcceleration.java | 5 ++--- .../fsensor/observer/SensorSubject.java | 2 +- .../fsensor/sensor/acceleration/AccelerationSensor.java | 6 +++--- .../ComplementaryLinearAccelerationSensor.java | 7 ++++--- .../acceleration/KalmanLinearAccelerationSensor.java | 7 ++++--- .../sensor/acceleration/LinearAccelerationSensor.java | 6 +++--- .../acceleration/LowPassLinearAccelerationSensor.java | 7 ++++--- .../sensor/gyroscope/ComplementaryGyroscopeSensor.java | 6 +++--- .../fsensor/sensor/gyroscope/GyroscopeSensor.java | 6 +++--- .../fsensor/sensor/gyroscope/KalmanGyroscopeSensor.java | 6 +++--- .../fsensor/util/gravity/GravityUtil.java | 3 --- .../kircherelectronics/fsensor/util/offset/FitPoints.java | 8 ++------ 16 files changed, 39 insertions(+), 44 deletions(-) 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 index fd569ff..56d551d 100644 --- 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 values; + private final ArrayDeque 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 index c7458b3..5d90a8f 100644 --- 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 values; + private final ArrayDeque values; private float[] output; diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java index 826498f..68412fc 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java @@ -61,15 +61,15 @@ public class OrientationFusedKalman extends OrientationFused { private static final String TAG = OrientationFusedComplementary.class.getSimpleName(); - private RotationKalmanFilter kalmanFilter; - private AtomicBoolean 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 rotationVectorAccelerationMagnetic; - private double[] vectorGyroscope = new double[4]; - private double[] vectorAccelerationMagnetic = new double[4]; + private final double[] vectorGyroscope = new double[4]; + private final double[] vectorAccelerationMagnetic = new double[4]; public OrientationFusedKalman() { this(DEFAULT_TIME_CONSTANT); diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/filter/RotationMeasurementModel.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/filter/RotationMeasurementModel.java index 9b013c7..fc8156d 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/filter/RotationMeasurementModel.java +++ b/fsensor/src/main/java/com/kircherelectronics/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/fsensor/src/main/java/com/kircherelectronics/fsensor/linearacceleration/LinearAcceleration.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/linearacceleration/LinearAcceleration.java index fd32b19..e1e0e6a 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/linearacceleration/LinearAcceleration.java +++ b/fsensor/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 index ec55a9b..13125b2 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/observer/SensorSubject.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/observer/SensorSubject.java @@ -4,7 +4,7 @@ import java.util.List; public class SensorSubject { - private List observers = new ArrayList<>(); + private final List observers = new ArrayList<>(); public interface SensorObserver { void onSensorChanged(float[] values); diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/AccelerationSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/AccelerationSensor.java index 24066bf..7381950 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/AccelerationSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/AccelerationSensor.java @@ -28,8 +28,8 @@ public class AccelerationSensor implements FSensor { private static final String TAG = AccelerationSensor.class.getSimpleName(); - private SensorManager sensorManager; - private SimpleSensorListener listener; + private final SensorManager sensorManager; + private final SimpleSensorListener listener; private float startTime = 0; private int count = 0; @@ -38,7 +38,7 @@ public class AccelerationSensor implements FSensor { private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; - private SensorSubject sensorSubject; + private final SensorSubject sensorSubject; public AccelerationSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/ComplementaryLinearAccelerationSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/ComplementaryLinearAccelerationSensor.java index bf89c35..a70331d 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/ComplementaryLinearAccelerationSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/ComplementaryLinearAccelerationSensor.java @@ -32,8 +32,8 @@ public class ComplementaryLinearAccelerationSensor implements FSensor { private static final String TAG = ComplementaryLinearAccelerationSensor.class.getSimpleName(); - private SensorManager sensorManager; - private SimpleSensorListener listener; + private final SensorManager sensorManager; + private final SimpleSensorListener listener; private float startTime = 0; private int count = 0; @@ -49,7 +49,7 @@ public class ComplementaryLinearAccelerationSensor implements FSensor { private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; private int sensorType = Sensor.TYPE_GYROSCOPE; - private SensorSubject sensorSubject; + private final SensorSubject sensorSubject; public ComplementaryLinearAccelerationSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); @@ -127,6 +127,7 @@ public void reset() { stop(); magnetic = new float[3]; acceleration = new float[3]; + rawAcceleration = new float[3]; rotation = new float[3]; output = new float[4]; listener.reset(); diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/KalmanLinearAccelerationSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/KalmanLinearAccelerationSensor.java index 18b29a2..8ba1f5a 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/KalmanLinearAccelerationSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/KalmanLinearAccelerationSensor.java @@ -32,8 +32,8 @@ public class KalmanLinearAccelerationSensor implements FSensor { private static final String TAG = KalmanLinearAccelerationSensor.class.getSimpleName(); - private SensorManager sensorManager; - private SimpleSensorListener listener; + private final SensorManager sensorManager; + private final SimpleSensorListener listener; private float startTime = 0; private int count = 0; @@ -49,7 +49,7 @@ public class KalmanLinearAccelerationSensor implements FSensor { private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; private int sensorType = Sensor.TYPE_GYROSCOPE; - private SensorSubject sensorSubject; + private final SensorSubject sensorSubject; public KalmanLinearAccelerationSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); @@ -117,6 +117,7 @@ public void reset() { stop(); magnetic = new float[3]; acceleration = new float[3]; + rawAcceleration = new float[3]; rotation = new float[3]; output = new float[4]; listener.reset(); diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LinearAccelerationSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LinearAccelerationSensor.java index 1a3794f..f33db0f 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LinearAccelerationSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LinearAccelerationSensor.java @@ -28,8 +28,8 @@ public class LinearAccelerationSensor implements FSensor { private static final String TAG = LinearAccelerationSensor.class.getSimpleName(); - private SensorManager sensorManager; - private SimpleSensorListener listener; + private final SensorManager sensorManager; + private final SimpleSensorListener listener; private float startTime = 0; private int count = 0; @@ -38,7 +38,7 @@ public class LinearAccelerationSensor implements FSensor { private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; - private SensorSubject sensorSubject; + private final SensorSubject sensorSubject; public LinearAccelerationSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LowPassLinearAccelerationSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LowPassLinearAccelerationSensor.java index 554a4a4..472c504 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LowPassLinearAccelerationSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/acceleration/LowPassLinearAccelerationSensor.java @@ -31,8 +31,8 @@ public class LowPassLinearAccelerationSensor implements FSensor { private static final String TAG = LowPassLinearAccelerationSensor.class.getSimpleName(); - private SensorManager sensorManager; - private SimpleSensorListener listener; + private final SensorManager sensorManager; + private final SimpleSensorListener listener; private float startTime = 0; private int count = 0; @@ -46,7 +46,7 @@ public class LowPassLinearAccelerationSensor implements FSensor { private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; - private SensorSubject sensorSubject; + private final SensorSubject sensorSubject; public LowPassLinearAccelerationSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); @@ -103,6 +103,7 @@ public void setFSensorLpfLinearAccelerationTimeConstant(float timeConstant) { public void reset() { stop(); acceleration = new float[3]; + rawAcceleration = new float[3]; output = new float[4]; start(); } diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/ComplementaryGyroscopeSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/ComplementaryGyroscopeSensor.java index bb6bcad..aa000fe 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/ComplementaryGyroscopeSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/ComplementaryGyroscopeSensor.java @@ -30,8 +30,8 @@ public class ComplementaryGyroscopeSensor implements FSensor { private static final String TAG = ComplementaryGyroscopeSensor.class.getSimpleName(); - private SensorManager sensorManager; - private SimpleSensorListener listener; + private final SensorManager sensorManager; + private final SimpleSensorListener listener; private float startTime = 0; private int count = 0; @@ -45,7 +45,7 @@ public class ComplementaryGyroscopeSensor implements FSensor { private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; private int sensorType = Sensor.TYPE_GYROSCOPE; - private SensorSubject sensorSubject; + private final SensorSubject sensorSubject; public ComplementaryGyroscopeSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/GyroscopeSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/GyroscopeSensor.java index 4c466fd..7247a6d 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/GyroscopeSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/GyroscopeSensor.java @@ -31,8 +31,8 @@ public class GyroscopeSensor implements FSensor { private static final String TAG = GyroscopeSensor.class.getSimpleName(); - private SensorManager sensorManager; - private SimpleSensorListener listener; + private final SensorManager sensorManager; + private final SimpleSensorListener listener; private float startTime = 0; private int count = 0; @@ -46,7 +46,7 @@ public class GyroscopeSensor implements FSensor { private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; private int sensorType = Sensor.TYPE_GYROSCOPE; - private SensorSubject sensorSubject; + private final SensorSubject sensorSubject; public GyroscopeSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/KalmanGyroscopeSensor.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/KalmanGyroscopeSensor.java index cd3952f..88c93d3 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/KalmanGyroscopeSensor.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/gyroscope/KalmanGyroscopeSensor.java @@ -30,8 +30,8 @@ public class KalmanGyroscopeSensor implements FSensor { private static final String TAG = KalmanGyroscopeSensor.class.getSimpleName(); - private SensorManager sensorManager; - private SimpleSensorListener listener; + private final SensorManager sensorManager; + private final SimpleSensorListener listener; private float startTime = 0; private int count = 0; @@ -45,7 +45,7 @@ public class KalmanGyroscopeSensor implements FSensor { private int sensorDelay = SensorManager.SENSOR_DELAY_FASTEST; private int sensorType = Sensor.TYPE_GYROSCOPE; - private SensorSubject sensorSubject; + private final SensorSubject sensorSubject; public KalmanGyroscopeSensor(Context context) { this.sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE); diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/util/gravity/GravityUtil.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/util/gravity/GravityUtil.java index 98d2ead..38665dc 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/util/gravity/GravityUtil.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/util/gravity/GravityUtil.java @@ -1,9 +1,6 @@ package com.kircherelectronics.fsensor.util.gravity; import android.hardware.SensorManager; -import android.util.Log; - -import java.util.Arrays; /* * Copyright 2018, Kircher Electronics, LLC diff --git a/fsensor/src/main/java/com/kircherelectronics/fsensor/util/offset/FitPoints.java b/fsensor/src/main/java/com/kircherelectronics/fsensor/util/offset/FitPoints.java index ad511d1..8d0574c 100644 --- a/fsensor/src/main/java/com/kircherelectronics/fsensor/util/offset/FitPoints.java +++ b/fsensor/src/main/java/com/kircherelectronics/fsensor/util/offset/FitPoints.java @@ -162,9 +162,7 @@ private RealVector solveSystem(ArrayList points) { RealMatrix dtdi = solver.getInverse(); // v = (( d' * d )^-1) * ( d' * ones.mapAddToSelf(1)); - RealVector v = dtdi.operate(dtOnes); - - return v; + return dtdi.operate(dtOnes); } /** @@ -245,9 +243,7 @@ private RealMatrix translateToCenter(RealVector center, RealMatrix a) { t.setSubMatrix(centerMatrix.getData(), 3, 0); // Translate to the offset. - RealMatrix r = t.multiply(a).multiply(t.transpose()); - - return r; + return t.multiply(a).multiply(t.transpose()); } /**