Skip to content

Commit 4ca9bc6

Browse files
author
crearo
committed
Remove the mHungary notation
Cuz damn it's an ugly mudafuka
1 parent 502c5d5 commit 4ca9bc6

File tree

9 files changed

+125
-149
lines changed

9 files changed

+125
-149
lines changed

.idea/caches/build_file_checksums.ser

590 Bytes
Binary file not shown.

.idea/caches/gradle_models.ser

121 KB
Binary file not shown.

.idea/codeStyles/Project.xml

Lines changed: 29 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

.idea/compiler.xml

Lines changed: 1 addition & 15 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

.idea/copyright/profiles_settings.xml

Lines changed: 0 additions & 3 deletions
This file was deleted.

.idea/misc.xml

Lines changed: 1 addition & 38 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

.idea/modules.xml

Lines changed: 1 addition & 1 deletion
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

.idea/vcs.xml

Lines changed: 6 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

sensorfusionlib/src/main/java/rish/crearo/sensorfusionlib/SensorFusion.java

Lines changed: 87 additions & 92 deletions
Original file line numberDiff line numberDiff line change
@@ -19,174 +19,169 @@
1919
public class SensorFusion implements SensorEventListener {
2020

2121
private static final String TAG = SensorFusion.class.getSimpleName();
22-
23-
private SensorManager mSensorManager;
24-
private Sensor mSensorGyro, mSensorAcc, mSensorMag;
25-
2622
/* Nanoseconds to seconds */
2723
private static final float NS2S = 1.0f / 1000000000.0f;
28-
29-
private long mPrevGyroTimestamp = 0;
24+
private SensorManager sensorManager;
25+
private Sensor sensorGyro, sensorAcc, sensorMag;
26+
private long prevGyroTimestamp = 0;
3027

3128
/**
3229
* This is the absolute difference in sensor values from the previously recorded reading.
3330
* For gyroscope, it is simply the integration of angular velocity (angVel * dT)
3431
* For acc/mag, it is the difference in orientation angles (YPR) obtained using
3532
* SensorManager.getOrientation(rotationMatrix) between current and prev orientations
3633
**/
37-
private float mGyroDiff[] = new float[3];
38-
private float mAccMagDiff[] = new float[3];
34+
private float gyroDiff[] = new float[3];
35+
private float accMagDiff[] = new float[3];
3936

4037
/**
4138
* Using these to clone values received from SensorEventListener callback
4239
*/
43-
private float mAccData[], mMagData[];
40+
private float accData[], magData[];
4441

4542
/**
4643
* Stores YPR(in that order) of the fused magnetometer and accelerometer obtained by calling
4744
* SensorManager.getRotationMatrix(acc, mag), and then passing the rotation matrix received to
4845
* SensorManager.getOrientation(rot).
4946
*/
50-
private float mAccMagPrevOrientation[];
51-
private float mAccMagOrientation[] = new float[3];
47+
private float accMagPrevOrientation[];
48+
private float accMagOrientation[] = new float[3];
5249

5350
/* temp variable storing rotation matrix */
54-
private float mAccMagRotationMatrix[] = new float[16];
51+
private float accMagRotationMatrix[] = new float[16];
5552

5653
/* pitch, roll, yaw (x, y, z) */
57-
private float mAccMagTrajectory[] = new float[3];
58-
private float mGyroTrajectoryCorrected[] = new float[3];
59-
private float mGyroTrajectoryRaw[] = new float[3];
60-
private float mFusedTrajectory[] = new float[3];
54+
private float accMagTrajectory[] = new float[3];
55+
private float gyroTrajectoryCorrected[] = new float[3];
56+
private float gyroTrajectoryRaw[] = new float[3];
57+
private float fusedTrajectory[] = new float[3];
6158

62-
private VerboseFusionListener mVerboseFusionListener;
63-
private FusionListener mFusionListener;
59+
private VerboseFusionListener verboseFusionListener;
60+
private FusionListener fusionListener;
61+
private float alpha = 0.995f;
62+
private float oneMinusAlpha = 1.0f - alpha;
6463

6564
public SensorFusion(Context context, VerboseFusionListener verboseFusionListener) {
6665
initSensors(context);
67-
mVerboseFusionListener = verboseFusionListener;
66+
this.verboseFusionListener = verboseFusionListener;
6867
}
6968

7069
public SensorFusion(Context context, FusionListener fusionListener) {
7170
initSensors(context);
72-
mFusionListener = fusionListener;
71+
this.fusionListener = fusionListener;
7372
}
7473

7574
private void initSensors(Context context) {
76-
mSensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
77-
mSensorGyro = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
78-
mSensorAcc = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
79-
mSensorMag = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
75+
sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
76+
sensorGyro = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
77+
sensorAcc = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
78+
sensorMag = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
8079
}
8180

8281
public void start() {
83-
mSensorManager.registerListener(this, mSensorGyro, SensorManager.SENSOR_DELAY_FASTEST);
84-
mSensorManager.registerListener(this, mSensorAcc, SensorManager.SENSOR_DELAY_FASTEST);
85-
mSensorManager.registerListener(this, mSensorMag, SensorManager.SENSOR_DELAY_FASTEST);
82+
sensorManager.registerListener(this, sensorGyro, SensorManager.SENSOR_DELAY_FASTEST);
83+
sensorManager.registerListener(this, sensorAcc, SensorManager.SENSOR_DELAY_FASTEST);
84+
sensorManager.registerListener(this, sensorMag, SensorManager.SENSOR_DELAY_FASTEST);
8685
}
8786

8887
public void stop() {
89-
mSensorManager.unregisterListener(this);
88+
sensorManager.unregisterListener(this);
9089
}
9190

9291
@Override
9392
public void onSensorChanged(SensorEvent event) {
94-
if (event.sensor.getType() == mSensorGyro.getType()) {
93+
if (event.sensor.getType() == sensorGyro.getType()) {
9594
calculateRawGyroOrientation(event.values, event.timestamp);
96-
if (mVerboseFusionListener != null)
97-
mVerboseFusionListener.onGyroOrientation(mGyroTrajectoryRaw, event.timestamp);
95+
if (verboseFusionListener != null)
96+
verboseFusionListener.onGyroOrientation(gyroTrajectoryRaw, event.timestamp);
9897
calculateFusedOrientation();
99-
if (mVerboseFusionListener != null)
100-
mVerboseFusionListener.onFusedOrientation(mFusedTrajectory, event.timestamp);
101-
if (mFusionListener != null)
102-
mFusionListener.onFusedOrientation(mFusedTrajectory, event.timestamp);
103-
} else if (event.sensor.getType() == mSensorMag.getType()) {
104-
if (mMagData == null) mMagData = new float[3];
105-
System.arraycopy(event.values, 0, mMagData, 0, event.values.length);
106-
} else if (event.sensor.getType() == mSensorAcc.getType()) {
107-
if (mAccData == null) mAccData = new float[3];
108-
System.arraycopy(event.values, 0, mAccData, 0, event.values.length);
98+
if (verboseFusionListener != null)
99+
verboseFusionListener.onFusedOrientation(fusedTrajectory, event.timestamp);
100+
if (fusionListener != null)
101+
fusionListener.onFusedOrientation(fusedTrajectory, event.timestamp);
102+
} else if (event.sensor.getType() == sensorMag.getType()) {
103+
if (magData == null) magData = new float[3];
104+
System.arraycopy(event.values, 0, magData, 0, event.values.length);
105+
} else if (event.sensor.getType() == sensorAcc.getType()) {
106+
if (accData == null) accData = new float[3];
107+
System.arraycopy(event.values, 0, accData, 0, event.values.length);
109108
calculateAccMagOrientation();
110-
if (mVerboseFusionListener != null)
111-
mVerboseFusionListener.onAccMagOrientation(mAccMagTrajectory, event.timestamp);
109+
if (verboseFusionListener != null)
110+
verboseFusionListener.onAccMagOrientation(accMagTrajectory, event.timestamp);
112111
}
113112
}
114113

115114
private void calculateRawGyroOrientation(float angularVelocity[], long timestamp) {
116-
if (mPrevGyroTimestamp != 0) {
117-
float dt = (timestamp - mPrevGyroTimestamp) * NS2S;
118-
mGyroDiff[0] = dt * angularVelocity[0];
119-
mGyroDiff[1] = dt * angularVelocity[1];
120-
mGyroDiff[2] = dt * angularVelocity[2];
115+
if (prevGyroTimestamp != 0) {
116+
float dt = (timestamp - prevGyroTimestamp) * NS2S;
117+
gyroDiff[0] = dt * angularVelocity[0];
118+
gyroDiff[1] = dt * angularVelocity[1];
119+
gyroDiff[2] = dt * angularVelocity[2];
121120
}
122-
mPrevGyroTimestamp = timestamp;
121+
prevGyroTimestamp = timestamp;
123122

124123
/* Add these diff values to raw and corrected trajectory, just the same */
125-
mGyroTrajectoryRaw[0] += mGyroDiff[0];
126-
mGyroTrajectoryRaw[1] += mGyroDiff[1];
127-
mGyroTrajectoryRaw[2] += mGyroDiff[2];
124+
gyroTrajectoryRaw[0] += gyroDiff[0];
125+
gyroTrajectoryRaw[1] += gyroDiff[1];
126+
gyroTrajectoryRaw[2] += gyroDiff[2];
128127

129-
mGyroTrajectoryCorrected[0] += mGyroDiff[0];
130-
mGyroTrajectoryCorrected[1] += mGyroDiff[1];
131-
mGyroTrajectoryCorrected[2] += mGyroDiff[2];
128+
gyroTrajectoryCorrected[0] += gyroDiff[0];
129+
gyroTrajectoryCorrected[1] += gyroDiff[1];
130+
gyroTrajectoryCorrected[2] += gyroDiff[2];
132131
}
133132

134133
private void calculateAccMagOrientation() {
135-
if (mMagData != null && mAccData != null) {
136-
if (SensorManager.getRotationMatrix(mAccMagRotationMatrix, null, mAccData, mMagData)) {
137-
SensorManager.getOrientation(mAccMagRotationMatrix, mAccMagOrientation);
138-
if (mAccMagPrevOrientation == null) {
139-
mAccMagPrevOrientation = new float[3];
140-
mAccMagDiff = new float[3];
134+
if (magData != null && accData != null) {
135+
if (SensorManager.getRotationMatrix(accMagRotationMatrix, null, accData, magData)) {
136+
SensorManager.getOrientation(accMagRotationMatrix, accMagOrientation);
137+
if (accMagPrevOrientation == null) {
138+
accMagPrevOrientation = new float[3];
139+
accMagDiff = new float[3];
141140
} else {
142-
mAccMagDiff[0] = mAccMagOrientation[0] - mAccMagPrevOrientation[0];
143-
mAccMagDiff[1] = mAccMagOrientation[1] - mAccMagPrevOrientation[1];
144-
mAccMagDiff[2] = mAccMagOrientation[2] - mAccMagPrevOrientation[2];
141+
accMagDiff[0] = accMagOrientation[0] - accMagPrevOrientation[0];
142+
accMagDiff[1] = accMagOrientation[1] - accMagPrevOrientation[1];
143+
accMagDiff[2] = accMagOrientation[2] - accMagPrevOrientation[2];
145144

146-
/**
147-
* Add these diff values to mAccMagTrajectory
145+
/* Add these diff values to accMagTrajectory
148146
* The getOrientation method returns value like so : -yaw, -pitch, roll (-z, -x, y)
149147
* To stay consistent throughout, I convert these here to the PRY (x,y,z)
150148
* I've followed throughout the code
151149
**/
152-
mAccMagTrajectory[0] -= mAccMagDiff[1]; // pitch
153-
mAccMagTrajectory[1] += mAccMagDiff[2]; // roll
154-
mAccMagTrajectory[2] -= mAccMagDiff[0]; // yaw
150+
accMagTrajectory[0] -= accMagDiff[1]; // pitch
151+
accMagTrajectory[1] += accMagDiff[2]; // roll
152+
accMagTrajectory[2] -= accMagDiff[0]; // yaw
155153
}
156154

157155
/* set cur values as prev */
158-
mAccMagPrevOrientation[0] = mAccMagOrientation[0];
159-
mAccMagPrevOrientation[1] = mAccMagOrientation[1];
160-
mAccMagPrevOrientation[2] = mAccMagOrientation[2];
156+
accMagPrevOrientation[0] = accMagOrientation[0];
157+
accMagPrevOrientation[1] = accMagOrientation[1];
158+
accMagPrevOrientation[2] = accMagOrientation[2];
161159
} else
162160
Log.e(TAG, "There was an error in calculating acc-mag orientation");
163161
}
164162
}
165163

166-
private float alpha = 0.995f;
167-
private float oneMinusAlpha = 1.0f - alpha;
168-
169164
private void calculateFusedOrientation() {
170-
mFusedTrajectory[0] = (alpha * mGyroTrajectoryCorrected[0]) + (oneMinusAlpha * mAccMagTrajectory[0]); // pitch
171-
mFusedTrajectory[1] = (alpha * mGyroTrajectoryCorrected[1]) + (oneMinusAlpha * mAccMagTrajectory[1]); // roll
172-
mFusedTrajectory[2] = (alpha * mGyroTrajectoryCorrected[2]) + (oneMinusAlpha * mAccMagTrajectory[2]); // yaw
173-
System.arraycopy(mFusedTrajectory, 0, mGyroTrajectoryCorrected, 0, mFusedTrajectory.length);
165+
fusedTrajectory[0] = (alpha * gyroTrajectoryCorrected[0]) + (oneMinusAlpha * accMagTrajectory[0]); // pitch
166+
fusedTrajectory[1] = (alpha * gyroTrajectoryCorrected[1]) + (oneMinusAlpha * accMagTrajectory[1]); // roll
167+
fusedTrajectory[2] = (alpha * gyroTrajectoryCorrected[2]) + (oneMinusAlpha * accMagTrajectory[2]); // yaw
168+
System.arraycopy(fusedTrajectory, 0, gyroTrajectoryCorrected, 0, fusedTrajectory.length);
174169
}
175170

176171
public void reset() {
177-
mGyroDiff = new float[3];
178-
mAccData = null;
179-
mMagData = null;
180-
181-
mAccMagPrevOrientation = null;
182-
mAccMagOrientation = new float[3];
183-
mAccMagDiff = new float[3];
184-
mAccMagRotationMatrix = new float[16];
185-
186-
mAccMagTrajectory = new float[3]; /* -yaw, -pitch, roll (-z, -x, y) */
187-
mGyroTrajectoryCorrected = new float[3]; /* pitch, roll, yaw (x, y, z) */
188-
mGyroTrajectoryRaw = new float[3]; /* pitch, roll, yaw (x, y, z) */
189-
mFusedTrajectory = new float[3]; /* pitch, roll, yaw (x, y, z) */
172+
gyroDiff = new float[3];
173+
accData = null;
174+
magData = null;
175+
176+
accMagPrevOrientation = null;
177+
accMagOrientation = new float[3];
178+
accMagDiff = new float[3];
179+
accMagRotationMatrix = new float[16];
180+
181+
accMagTrajectory = new float[3]; /* -yaw, -pitch, roll (-z, -x, y) */
182+
gyroTrajectoryCorrected = new float[3]; /* pitch, roll, yaw (x, y, z) */
183+
gyroTrajectoryRaw = new float[3]; /* pitch, roll, yaw (x, y, z) */
184+
fusedTrajectory = new float[3]; /* pitch, roll, yaw (x, y, z) */
190185
}
191186

192187
@Override

0 commit comments

Comments
 (0)