Skip to content

Commit 54482a1

Browse files
KalebKEKalebKE
authored
Merge pull request #33 from KalebKE/2.0
2.0
2 parents 0ac19df + 93d0aed commit 54482a1

22 files changed

+556
-305
lines changed

‎fsensor/build.gradle

+2-5
Original file line numberDiff line numberDiff line change
@@ -14,8 +14,8 @@ android {
1414
defaultConfig {
1515
minSdkVersion 14
1616
targetSdkVersion 29
17-
versionCode 8
18-
versionName "1.2.3"
17+
versionCode 9
18+
versionName "2.0"
1919

2020
testInstrumentationRunner "android.support.test.runner.AndroidJUnitRunner"
2121

@@ -38,7 +38,4 @@ dependencies {
3838
testImplementation 'junit:junit:4.12'
3939

4040
api files('libs/commons-math3-3.6.1.jar')
41-
42-
api 'io.reactivex.rxjava2:rxandroid:2.1.0'
43-
api 'io.reactivex.rxjava2:rxjava:2.2.2'
4441
}

‎fsensor/src/main/java/com/kircherelectronics/fsensor/filter/averaging/MeanFilter.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,7 @@ public class MeanFilter extends AveragingFilter {
3737

3838
private static final String tag = MeanFilter.class.getSimpleName();
3939

40-
private ArrayDeque<float[]> values;
40+
private final ArrayDeque<float[]> values;
4141
private float[] output;
4242

4343
/**

‎fsensor/src/main/java/com/kircherelectronics/fsensor/filter/averaging/MedianFilter.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ public class MedianFilter extends AveragingFilter {
4242
private static final String tag = MedianFilter.class
4343
.getSimpleName();
4444

45-
private ArrayDeque<float[]> values;
45+
private final ArrayDeque<float[]> values;
4646
private float[] output;
4747

4848

‎fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/OrientationFused.java

+6-6
Original file line numberDiff line numberDiff line change
@@ -36,9 +36,9 @@ public abstract class OrientationFused extends BaseFilter {
3636
// transfer functions (rotations from the gyroscope and
3737
// acceleration/magnetic, respectively).
3838
public float timeConstant;
39-
protected Quaternion rotationVectorGyroscope;
39+
protected Quaternion rotationVector;
4040
protected long timestamp = 0;
41-
protected float[] output;
41+
protected float[] output = new float[3];
4242

4343
/**
4444
* Initialize a singleton instance.
@@ -49,7 +49,7 @@ public OrientationFused() {
4949

5050
public OrientationFused(float timeConstant) {
5151
this.timeConstant = timeConstant;
52-
output = new float[3];
52+
5353
}
5454

5555
@Override
@@ -59,11 +59,11 @@ public float[] getOutput() {
5959

6060
public void reset() {
6161
timestamp = 0;
62-
rotationVectorGyroscope = null;
62+
rotationVector = null;
6363
}
6464

6565
public boolean isBaseOrientationSet() {
66-
return rotationVectorGyroscope != null;
66+
return rotationVector != null;
6767
}
6868

6969
/**
@@ -87,6 +87,6 @@ public void setTimeConstant(float timeConstant) {
8787
public abstract float[] calculateFusedOrientation(float[] gyroscope, long timestamp, float[] acceleration, float[] magnetic);
8888

8989
public void setBaseOrientation(Quaternion baseOrientation) {
90-
rotationVectorGyroscope = baseOrientation;
90+
rotationVector = baseOrientation;
9191
}
9292
}

‎fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/complementary/OrientationFusedComplementary.java

+8-9
Original file line numberDiff line numberDiff line change
@@ -120,23 +120,22 @@ public float[] calculateFusedOrientation(float[] gyroscope, long timestamp, floa
120120
Quaternion rotationVectorAccelerationMagnetic = RotationUtil.getOrientationVectorFromAccelerationMagnetic(acceleration, magnetic);
121121

122122
if(rotationVectorAccelerationMagnetic != null) {
123-
rotationVectorGyroscope = RotationUtil.integrateGyroscopeRotation(rotationVectorGyroscope, gyroscope, dT, EPSILON);
123+
124+
rotationVector = RotationUtil.integrateGyroscopeRotation(rotationVector, gyroscope, dT, EPSILON);
124125

125126
// Apply the complementary fusedOrientation. // We multiply each rotation by their
126127
// coefficients (scalar matrices)...
127-
Quaternion scaledRotationVectorAccelerationMagnetic = rotationVectorAccelerationMagnetic.multiply
128-
(oneMinusAlpha);
128+
Quaternion scaledRotationVectorAccelerationMagnetic = rotationVectorAccelerationMagnetic.multiply(oneMinusAlpha);
129129

130130
// Scale our quaternion for the gyroscope
131-
Quaternion scaledRotationVectorGyroscope = rotationVectorGyroscope.multiply(alpha);
131+
Quaternion scaledRotationVectorGyroscope = rotationVector.multiply(alpha);
132132

133-
// ...and then add the two quaternions together.
133+
//...and then add the two quaternions together.
134134
// output[0] = alpha * output[0] + (1 - alpha) * input[0];
135-
rotationVectorGyroscope = scaledRotationVectorGyroscope.add
136-
(scaledRotationVectorAccelerationMagnetic);
137-
}
135+
Quaternion result = scaledRotationVectorGyroscope.add(scaledRotationVectorAccelerationMagnetic);
138136

139-
output = AngleUtils.getAngles(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(), rotationVectorGyroscope.getQ3());
137+
output = AngleUtils.getAngles(result.getQ0(), result.getQ1(), result.getQ2(), result.getQ3());
138+
}
140139
}
141140

142141
this.timestamp = timestamp;

‎fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/OrientationFusedKalman.java

+28-33
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
import org.apache.commons.math3.complex.Quaternion;
1414

1515
import java.util.Arrays;
16+
import java.util.concurrent.atomic.AtomicBoolean;
1617

1718
/*
1819
* Copyright 2018, Kircher Electronics, LLC
@@ -60,38 +61,41 @@ public class OrientationFusedKalman extends OrientationFused {
6061

6162
private static final String TAG = OrientationFusedComplementary.class.getSimpleName();
6263

63-
private RotationKalmanFilter kalmanFilter;
64-
private volatile boolean run;
64+
private final RotationKalmanFilter kalmanFilter;
65+
private final AtomicBoolean run;
6566
private volatile float dT;
6667
private volatile float[] output = new float[3];
6768
private Thread thread;
6869

69-
private volatile Quaternion rotationOrientation;
70+
private volatile Quaternion rotationVectorAccelerationMagnetic;
71+
private final double[] vectorGyroscope = new double[4];
72+
private final double[] vectorAccelerationMagnetic = new double[4];
7073

7174
public OrientationFusedKalman() {
7275
this(DEFAULT_TIME_CONSTANT);
7376
}
7477

7578
public OrientationFusedKalman(float timeConstant) {
7679
super(timeConstant);
80+
run = new AtomicBoolean(false);
7781
kalmanFilter = new RotationKalmanFilter(new RotationProcessModel(), new RotationMeasurementModel());
7882
}
7983

8084
public void startFusion() {
81-
if (!run && thread == null) {
82-
run = true;
85+
if (!run.get() && thread == null) {
86+
run.set(true);
8387

8488
thread = new Thread(new Runnable() {
8589
@Override
8690
public void run() {
87-
while (run && !Thread.interrupted()) {
91+
while (run.get() && !Thread.interrupted()) {
8892

89-
calculate();
93+
output = calculate();
9094

9195
try {
9296
Thread.sleep(20);
9397
} catch (InterruptedException e) {
94-
Log.e(TAG, "Kalman Thread Run", e);
98+
Log.e(TAG, "Kalman Thread", e);
9599
Thread.currentThread().interrupt();
96100
}
97101
}
@@ -105,8 +109,8 @@ public void run() {
105109
}
106110

107111
public void stopFusion() {
108-
if (run && thread != null) {
109-
run = false;
112+
if (run.get() && thread != null) {
113+
run.set(false);
110114
thread.interrupt();
111115
thread = null;
112116
}
@@ -133,21 +137,16 @@ public float[] getOutput() {
133137
* @return An orientation vector -> @link SensorManager#getOrientation(float[], float[])}
134138
*/
135139
private float[] calculate() {
136-
if (rotationVectorGyroscope != null && rotationOrientation != null && dT != 0) {
140+
if (rotationVector != null && rotationVectorAccelerationMagnetic != null && dT != 0) {
141+
vectorGyroscope[0] = (float) rotationVector.getVectorPart()[0];
142+
vectorGyroscope[1] = (float) rotationVector.getVectorPart()[1];
143+
vectorGyroscope[2] = (float) rotationVector.getVectorPart()[2];
144+
vectorGyroscope[3] = (float) rotationVector.getScalarPart();
137145

138-
double[] vectorGyroscope = new double[4];
139-
140-
vectorGyroscope[0] = (float) rotationVectorGyroscope.getVectorPart()[0];
141-
vectorGyroscope[1] = (float) rotationVectorGyroscope.getVectorPart()[1];
142-
vectorGyroscope[2] = (float) rotationVectorGyroscope.getVectorPart()[2];
143-
vectorGyroscope[3] = (float) rotationVectorGyroscope.getScalarPart();
144-
145-
double[] vectorAccelerationMagnetic = new double[4];
146-
147-
vectorAccelerationMagnetic[0] = (float) rotationOrientation.getVectorPart()[0];
148-
vectorAccelerationMagnetic[1] = (float) rotationOrientation.getVectorPart()[1];
149-
vectorAccelerationMagnetic[2] = (float) rotationOrientation.getVectorPart()[2];
150-
vectorAccelerationMagnetic[3] = (float) rotationOrientation.getScalarPart();
146+
vectorAccelerationMagnetic[0] = (float) rotationVectorAccelerationMagnetic.getVectorPart()[0];
147+
vectorAccelerationMagnetic[1] = (float) rotationVectorAccelerationMagnetic.getVectorPart()[1];
148+
vectorAccelerationMagnetic[2] = (float) rotationVectorAccelerationMagnetic.getVectorPart()[2];
149+
vectorAccelerationMagnetic[3] = (float) rotationVectorAccelerationMagnetic.getScalarPart();
151150

152151
// Apply the Kalman fusedOrientation... Note that the prediction and correction
153152
// inputs could be swapped, but the fusedOrientation is much more stable in this
@@ -156,15 +155,12 @@ private float[] calculate() {
156155
kalmanFilter.correct(vectorAccelerationMagnetic);
157156

158157
// rotation estimation.
159-
rotationVectorGyroscope = new Quaternion(kalmanFilter.getStateEstimation()[3],
160-
Arrays.copyOfRange(kalmanFilter.getStateEstimation(), 0, 3));
161-
162-
output = AngleUtils.getAngles(rotationVectorGyroscope.getQ0(), rotationVectorGyroscope.getQ1(), rotationVectorGyroscope.getQ2(), rotationVectorGyroscope.getQ3());
158+
Quaternion result = new Quaternion(kalmanFilter.getStateEstimation()[3], Arrays.copyOfRange(kalmanFilter.getStateEstimation(), 0, 3));
163159

164-
return output;
160+
output = AngleUtils.getAngles(result.getQ0(), result.getQ1(), result.getQ2(), result.getQ3());
165161
}
166162

167-
return null;
163+
return output;
168164
}
169165

170166
/**
@@ -177,13 +173,12 @@ private float[] calculate() {
177173
* @return the fused orientation estimation.
178174
*/
179175
public float[] calculateFusedOrientation(float[] gyroscope, long timestamp, float[] acceleration, float[] magnetic) {
180-
181176
if(isBaseOrientationSet()) {
182177
if (this.timestamp != 0) {
183178
dT = (timestamp - this.timestamp) * NS2S;
184179

185-
rotationOrientation = RotationUtil.getOrientationVectorFromAccelerationMagnetic(acceleration, magnetic);
186-
rotationVectorGyroscope = RotationUtil.integrateGyroscopeRotation(rotationVectorGyroscope, gyroscope, dT, EPSILON);
180+
rotationVectorAccelerationMagnetic = RotationUtil.getOrientationVectorFromAccelerationMagnetic(acceleration, magnetic);
181+
rotationVector = RotationUtil.integrateGyroscopeRotation(rotationVector, gyroscope, dT, EPSILON);
187182
}
188183
this.timestamp = timestamp;
189184

‎fsensor/src/main/java/com/kircherelectronics/fsensor/filter/gyroscope/fusion/kalman/filter/RotationMeasurementModel.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@
2121
*/
2222
public class RotationMeasurementModel implements MeasurementModel
2323
{
24-
private double noiseCoefficient = 0.001;
24+
private final double noiseCoefficient = 0.001;
2525

2626
/**
2727
* The measurement matrix, used to associate the measurement vector to the

‎fsensor/src/main/java/com/kircherelectronics/fsensor/linearacceleration/LinearAcceleration.java

+2-3
Original file line numberDiff line numberDiff line change
@@ -30,10 +30,9 @@ public abstract class LinearAcceleration {
3030

3131
private static final String tag = LinearAcceleration.class.getSimpleName();
3232

33-
private float[] output = new float[]
34-
{0, 0, 0};
33+
private final float[] output = new float[]{0, 0, 0};
3534

36-
protected BaseFilter filter;
35+
protected final BaseFilter filter;
3736

3837
public LinearAcceleration(BaseFilter filter) {
3938
this.filter = filter;

‎fsensor/src/main/java/com/kircherelectronics/fsensor/observer/SensorSubject.java

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
package com.kircherelectronics.fsensor.observer;
2+
3+
import java.util.ArrayList;
4+
import java.util.List;
5+
6+
public class SensorSubject {
7+
private final List<SensorObserver> observers = new ArrayList<>();
8+
9+
public interface SensorObserver {
10+
void onSensorChanged(float[] values);
11+
}
12+
13+
public void register(SensorObserver observer) {
14+
if(!observers.contains(observer)) {
15+
observers.add(observer);
16+
}
17+
}
18+
19+
public void unregister(SensorObserver observer) {
20+
observers.remove(observer);
21+
}
22+
23+
public void onNext(float[] values) {
24+
for(int i = 0; i < observers.size(); i++) {
25+
observers.get(i).onSensorChanged(values);
26+
}
27+
}
28+
}

‎fsensor/src/main/java/com/kircherelectronics/fsensor/sensor/FSensor.java

+6-2
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
package com.kircherelectronics.fsensor.sensor;
22

3-
import io.reactivex.subjects.PublishSubject;
3+
import com.kircherelectronics.fsensor.observer.SensorSubject;
44

55
/*
66
* Copyright 2018, Kircher Electronics, LLC
@@ -19,5 +19,9 @@
1919
*/
2020

2121
public interface FSensor {
22-
PublishSubject<float[]> getPublishSubject();
22+
void register(SensorSubject.SensorObserver sensorObserver);
23+
void unregister(SensorSubject.SensorObserver sensorObserver);
24+
25+
void start();
26+
void stop();
2327
}

0 commit comments

Comments
 (0)