|
19 | 19 | public class SensorFusion implements SensorEventListener {
|
20 | 20 |
|
21 | 21 | private static final String TAG = SensorFusion.class.getSimpleName();
|
22 |
| - |
23 |
| - private SensorManager mSensorManager; |
24 |
| - private Sensor mSensorGyro, mSensorAcc, mSensorMag; |
25 |
| - |
26 | 22 | /* Nanoseconds to seconds */
|
27 | 23 | 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; |
30 | 27 |
|
31 | 28 | /**
|
32 | 29 | * This is the absolute difference in sensor values from the previously recorded reading.
|
33 | 30 | * For gyroscope, it is simply the integration of angular velocity (angVel * dT)
|
34 | 31 | * For acc/mag, it is the difference in orientation angles (YPR) obtained using
|
35 | 32 | * SensorManager.getOrientation(rotationMatrix) between current and prev orientations
|
36 | 33 | **/
|
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]; |
39 | 36 |
|
40 | 37 | /**
|
41 | 38 | * Using these to clone values received from SensorEventListener callback
|
42 | 39 | */
|
43 |
| - private float mAccData[], mMagData[]; |
| 40 | + private float accData[], magData[]; |
44 | 41 |
|
45 | 42 | /**
|
46 | 43 | * Stores YPR(in that order) of the fused magnetometer and accelerometer obtained by calling
|
47 | 44 | * SensorManager.getRotationMatrix(acc, mag), and then passing the rotation matrix received to
|
48 | 45 | * SensorManager.getOrientation(rot).
|
49 | 46 | */
|
50 |
| - private float mAccMagPrevOrientation[]; |
51 |
| - private float mAccMagOrientation[] = new float[3]; |
| 47 | + private float accMagPrevOrientation[]; |
| 48 | + private float accMagOrientation[] = new float[3]; |
52 | 49 |
|
53 | 50 | /* temp variable storing rotation matrix */
|
54 |
| - private float mAccMagRotationMatrix[] = new float[16]; |
| 51 | + private float accMagRotationMatrix[] = new float[16]; |
55 | 52 |
|
56 | 53 | /* 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]; |
61 | 58 |
|
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; |
64 | 63 |
|
65 | 64 | public SensorFusion(Context context, VerboseFusionListener verboseFusionListener) {
|
66 | 65 | initSensors(context);
|
67 |
| - mVerboseFusionListener = verboseFusionListener; |
| 66 | + this.verboseFusionListener = verboseFusionListener; |
68 | 67 | }
|
69 | 68 |
|
70 | 69 | public SensorFusion(Context context, FusionListener fusionListener) {
|
71 | 70 | initSensors(context);
|
72 |
| - mFusionListener = fusionListener; |
| 71 | + this.fusionListener = fusionListener; |
73 | 72 | }
|
74 | 73 |
|
75 | 74 | 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); |
80 | 79 | }
|
81 | 80 |
|
82 | 81 | 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); |
86 | 85 | }
|
87 | 86 |
|
88 | 87 | public void stop() {
|
89 |
| - mSensorManager.unregisterListener(this); |
| 88 | + sensorManager.unregisterListener(this); |
90 | 89 | }
|
91 | 90 |
|
92 | 91 | @Override
|
93 | 92 | public void onSensorChanged(SensorEvent event) {
|
94 |
| - if (event.sensor.getType() == mSensorGyro.getType()) { |
| 93 | + if (event.sensor.getType() == sensorGyro.getType()) { |
95 | 94 | 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); |
98 | 97 | 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); |
109 | 108 | calculateAccMagOrientation();
|
110 |
| - if (mVerboseFusionListener != null) |
111 |
| - mVerboseFusionListener.onAccMagOrientation(mAccMagTrajectory, event.timestamp); |
| 109 | + if (verboseFusionListener != null) |
| 110 | + verboseFusionListener.onAccMagOrientation(accMagTrajectory, event.timestamp); |
112 | 111 | }
|
113 | 112 | }
|
114 | 113 |
|
115 | 114 | 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]; |
121 | 120 | }
|
122 |
| - mPrevGyroTimestamp = timestamp; |
| 121 | + prevGyroTimestamp = timestamp; |
123 | 122 |
|
124 | 123 | /* 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]; |
128 | 127 |
|
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]; |
132 | 131 | }
|
133 | 132 |
|
134 | 133 | 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]; |
141 | 140 | } 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]; |
145 | 144 |
|
146 |
| - /** |
147 |
| - * Add these diff values to mAccMagTrajectory |
| 145 | + /* Add these diff values to accMagTrajectory |
148 | 146 | * The getOrientation method returns value like so : -yaw, -pitch, roll (-z, -x, y)
|
149 | 147 | * To stay consistent throughout, I convert these here to the PRY (x,y,z)
|
150 | 148 | * I've followed throughout the code
|
151 | 149 | **/
|
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 |
155 | 153 | }
|
156 | 154 |
|
157 | 155 | /* 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]; |
161 | 159 | } else
|
162 | 160 | Log.e(TAG, "There was an error in calculating acc-mag orientation");
|
163 | 161 | }
|
164 | 162 | }
|
165 | 163 |
|
166 |
| - private float alpha = 0.995f; |
167 |
| - private float oneMinusAlpha = 1.0f - alpha; |
168 |
| - |
169 | 164 | 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); |
174 | 169 | }
|
175 | 170 |
|
176 | 171 | 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) */ |
190 | 185 | }
|
191 | 186 |
|
192 | 187 | @Override
|
|
0 commit comments