├── AndroidManifest.xml
├── README.md
├── ic_launcher-web.png
├── java
    └── com
    │   └── goatstone
    │       ├── sensorFusion
    │           ├── BubbleLevelCompass.java
    │           └── MainActivity.java
    │       └── util
    │           └── SensorFusion.java
└── res
    ├── drawable-ldpi
        └── ic_launcher.png
    ├── drawable-mdpi
        └── ic_launcher.png
    ├── ic_launcher
        └── ic_launcher.png
    ├── layout
        └── main.xml
    └── values
        └── strings.xml


/AndroidManifest.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" encoding="utf-8"?>
 2 | <manifest xmlns:android="http://schemas.android.com/apk/res/android"
 3 |     package="com.goatstone.sensorFusion"
 4 |     android:versionCode="1"
 5 |     android:versionName="1.0" >
 6 | 
 7 |     <uses-sdk android:minSdkVersion="10" />
 8 | 
 9 |     <application
10 |         android:icon="@drawable/ic_launcher"
11 |         android:label="@string/app_name" >
12 |         <activity
13 |             android:screenOrientation="portrait"
14 |             android:name=".MainActivity"
15 |             android:label="@string/app_name" >
16 |             <intent-filter>
17 |                 <action android:name="android.intent.action.MAIN" />
18 | 
19 |                 <category android:name="android.intent.category.LAUNCHER" />
20 |             </intent-filter>
21 |         </activity>
22 |     </application>
23 | 
24 | </manifest>


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | AndroidSensorFusion  
 2 | com.goatstone.util.SensorFusion
 3 | ===================
 4 | 
 5 | Get, fuse and display sensor information for Android.
 6 | 
 7 | Usage:
 8 | 
 9 |     private SensorFusion sensorFusion;
10 |     sensorFusion = new SensorFusion();
11 |     sensorFusion.setMode(SensorFusion.Mode.ACC_MAG);
12 | 
13 |     double azimuthValue = sensorFusion.getAzimuth();
14 |     double rollValue =  sensorFusion.getRoll();
15 |     double pitchValue =  sensorFusion.getPitch();
16 | 


--------------------------------------------------------------------------------
/ic_launcher-web.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/goatstone/AndroidSensorFusion/2fc70e56c24efc229cd4d5327d8e65b2bb6e6753/ic_launcher-web.png


--------------------------------------------------------------------------------
/java/com/goatstone/sensorFusion/BubbleLevelCompass.java:
--------------------------------------------------------------------------------
  1 | /************************************************************************************
  2 |  * Copyright (c) 2014 Jose Collas
  3 |  *
  4 |  * Permission is hereby granted, free of charge, to any person obtaining
  5 |  * a copy of this software and associated documentation files (the "Software"),
  6 |  * to deal in the Software without restriction, including without limitation
  7 |  * the rights to use, copy, modify, merge, publish, distribute, sublicense,
  8 |  * and/or sell copies of the Software, and to permit persons to whom the
  9 |  * Software is furnished to do so, subject to the following conditions:
 10 |  *
 11 |  *
 12 |  * The above copyright notice and this permission notice shall be included
 13 |  * in all copies or substantial portions of the Software.
 14 |  *
 15 |  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 16 |  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 17 |  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 18 |  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 19 |  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 20 |  * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE
 21 |  * OR OTHER DEALINGS IN THE SOFTWARE.
 22 |  ************************************************************************************/
 23 | package com.goatstone.sensorFusion;
 24 | 
 25 | 
 26 | import android.content.Context;
 27 | import android.graphics.Canvas;
 28 | import android.graphics.Color;
 29 | import android.graphics.Paint;
 30 | import android.os.Handler;
 31 | import android.util.AttributeSet;
 32 | import android.util.Log;
 33 | import android.view.View;
 34 | import android.view.ViewGroup;
 35 | 
 36 | import java.util.Date;
 37 | 
 38 | /**
 39 |  * BubbleLevelCompass Widget : Takes input from sensors and displays a bubble level and compass.
 40 |  * @author Jose Collas
 41 |  *
 42 |  * @version 0.7 1/26/14.
 43 |  */
 44 | public class BubbleLevelCompass extends ViewGroup {
 45 | 
 46 |     public Pointer pointer;
 47 | 
 48 |     private Paint mainPaint;
 49 |     private String message = "";
 50 | 
 51 |     public BubbleLevelCompass(Context context) {
 52 |         super(context);
 53 |         float pointerRadius = 12.0f;
 54 |         requestFocus();
 55 | 
 56 |         // Set up the paint for the label text
 57 |         mainPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
 58 |         mainPaint.setTextSize(20);
 59 |         mainPaint.setColor(Color.BLUE);
 60 |         pointer = new Pointer(getContext());
 61 |         pointer.setPointerRadius((int) pointerRadius);
 62 |         pointer.setPointerColor(Color.GRAY);
 63 | 
 64 |         addView(pointer);
 65 |     }
 66 | 
 67 |     public BubbleLevelCompass(Context context, AttributeSet attrs) {
 68 |         super(context, attrs);
 69 |         float pointerRadius = 12.0f;
 70 |         requestFocus();
 71 |         setPadding(20, 20, 20, 20);
 72 |         setBackgroundColor(Color.GRAY);
 73 | 
 74 |         mainPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
 75 |         mainPaint.setTextSize(20);
 76 |         mainPaint.setColor(Color.BLUE);
 77 |         pointer = new Pointer(getContext());
 78 |         pointer.setPointerRadius((int) pointerRadius);
 79 |         addView(pointer);
 80 | 
 81 |     }
 82 | 
 83 |     /**
 84 |      * Pointer : represents a UI element that will point to another UI element in order to select it
 85 |      */
 86 |     private class Pointer extends View {
 87 | 
 88 |         private int pointerRadius = 22;
 89 |         private int left = 200;
 90 |         private int top = 120;
 91 |         private Paint paint = new Paint(Paint.ANTI_ALIAS_FLAG);
 92 |         private Paint pointerPaint = new Paint(Paint.ANTI_ALIAS_FLAG);
 93 |         private int azimuth;
 94 |         int viewWidth;
 95 |         int viewHeight;
 96 |         int distanceMult = 1;
 97 | 
 98 |         public void setPointerRadius(int r) {
 99 |             pointerRadius = r;
100 |         }
101 | 
102 |         public void setPointerColor(int color) {
103 |             paint.setColor(color);
104 |             invalidate();
105 |         }
106 | 
107 |         public void setPointerLeft(int left) {
108 |             this.left = (left * distanceMult);
109 |             invalidate();
110 |         }
111 | 
112 |         public int getPointerLeft() {
113 |             return left;
114 |         }
115 | 
116 |         public void setPointerTop(int top) {
117 |             this.top = (top * distanceMult);
118 |             invalidate();
119 |         }
120 | 
121 |         public void setAzimuth(int azimuth) {
122 |             this.azimuth = azimuth;
123 |         }
124 | 
125 |         //private Handler mainHandler;
126 |         private Handler handler;
127 | 
128 |         public Pointer(Context context) {
129 |             super(context);
130 |             paint.setStrokeWidth(6.0f);
131 |             paint.setColor(Color.argb(250, 0, 0, 250));
132 | 
133 |             pointerPaint.setStrokeWidth(3.0f);
134 |             pointerPaint.setColor(Color.argb(200, 250, 10, 10));
135 |             viewWidth = getWidth();
136 |             viewHeight = getHeight();
137 |             left = viewWidth / 2;
138 |             top = viewHeight / 2;
139 |             setBackgroundColor(Color.argb(200, 250, 200, 200));
140 | 
141 | //            handler = new Handler();
142 | //            handler.postDelayed(runnable, 100);
143 |         }
144 | 
145 |         private Runnable runnable = new Runnable() {
146 |             @Override
147 |             public void run() {
148 |                 Log.i("tag", "run: " + new Date());
149 |                 //invalidate();
150 |                 handler.postDelayed(this, 1000);
151 |             }
152 |         };
153 | 
154 |         @Override
155 |         protected void onDraw(Canvas canvas) {
156 |             int viewWidth = getWidth();
157 |             int viewHeight = getHeight();
158 | 
159 |             canvas.translate((viewWidth / 2), (viewHeight / 2));
160 |             canvas.drawCircle(0 + left, 0 + top - 150, pointerRadius, pointerPaint);
161 |             canvas.rotate(azimuth * -1 - 90);
162 |             canvas.drawLine(0, 0, 50, 0, paint);
163 | 
164 |         }
165 |     } // end Pointer
166 | 
167 |     public void setPLeft(int left) {
168 |         pointer.setPointerLeft(left);
169 |     }
170 | 
171 |     public void setPTop(int top) {
172 |         pointer.setPointerTop(top * -1);
173 |     }
174 | 
175 |     public void setMessage(String message) {
176 |         this.message = message;
177 |         invalidate();
178 |     }
179 | 
180 |     public void setAzimuth(int azimuth1) {
181 |         pointer.setAzimuth(azimuth1);
182 |     }
183 | 
184 |     @Override
185 |     protected void onLayout(boolean changed, int l, int t, int r, int b) {
186 |         // Do nothing. Do not call the superclass method--that would start a layout pass
187 |         // on this view's children. ?? - RoundDial lays out its children in onSizeChanged(). ??
188 |     }
189 | 
190 |     @Override
191 |     protected void onDraw(Canvas canvas) {
192 |         super.onDraw(canvas);
193 | //        canvas.drawText("CustomView : pointer.getPointerLeft() : " + pointer.getPointerLeft(), 10, 300, mainPaint);
194 |     }
195 | 
196 |     @Override
197 |     protected int getSuggestedMinimumWidth() {
198 |         return 400;
199 |     }
200 | 
201 |     @Override
202 |     protected int getSuggestedMinimumHeight() {
203 |         return (int) 400;
204 |     }
205 | 
206 |     @Override
207 |     protected void onMeasure(int widthMeasureSpec, int heightMeasureSpec) {
208 |         setMeasuredDimension(400, 500);
209 |     }
210 | 
211 |     @Override
212 |     protected void onSizeChanged(int w, int h, int oldw, int oldh) {
213 |         super.onSizeChanged(w, h, oldw, oldh);
214 |         pointer.layout(0, 0, w, h);
215 |     }
216 | 
217 | }
218 | 


--------------------------------------------------------------------------------
/java/com/goatstone/sensorFusion/MainActivity.java:
--------------------------------------------------------------------------------
  1 | /************************************************************************************
  2 |  * Copyright (c) 2014 Jose Collas
  3 |  *
  4 |  * Permission is hereby granted, free of charge, to any person obtaining
  5 |  * a copy of this software and associated documentation files (the "Software"),
  6 |  * to deal in the Software without restriction, including without limitation
  7 |  * the rights to use, copy, modify, merge, publish, distribute, sublicense,
  8 |  * and/or sell copies of the Software, and to permit persons to whom the
  9 |  * Software is furnished to do so, subject to the following conditions:
 10 |  *
 11 |  *
 12 |  * The above copyright notice and this permission notice shall be included
 13 |  * in all copies or substantial portions of the Software.
 14 |  *
 15 |  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 16 |  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 17 |  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 18 |  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 19 |  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 20 |  * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE
 21 |  * OR OTHER DEALINGS IN THE SOFTWARE.
 22 |  ************************************************************************************/
 23 | package com.goatstone.sensorFusion;
 24 | 
 25 | import android.app.Activity;
 26 | import android.hardware.Sensor;
 27 | import android.hardware.SensorEvent;
 28 | import android.hardware.SensorEventListener;
 29 | import android.hardware.SensorManager;
 30 | import android.os.Bundle;
 31 | import android.widget.RadioGroup;
 32 | import android.widget.TextView;
 33 | 
 34 | import com.goatstone.util.SensorFusion;
 35 | 
 36 | import java.text.DecimalFormat;
 37 | 
 38 | public class MainActivity extends Activity
 39 |         implements SensorEventListener, RadioGroup.OnCheckedChangeListener {
 40 | 
 41 |     private SensorFusion sensorFusion;
 42 |     private BubbleLevelCompass bubbleLevelCompass;
 43 |     private SensorManager sensorManager = null;
 44 | 
 45 |     private RadioGroup setModeRadioGroup;
 46 |     private TextView azimuthText, pithText, rollText;
 47 |     private DecimalFormat d = new DecimalFormat("#.##");
 48 | 
 49 |     @Override
 50 |     public void onCreate(Bundle savedInstanceState) {
 51 |         super.onCreate(savedInstanceState);
 52 |         setContentView(R.layout.main);
 53 | 
 54 |         sensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
 55 |         registerSensorManagerListeners();
 56 | 
 57 |         d.setMaximumFractionDigits(2);
 58 |         d.setMinimumFractionDigits(2);
 59 | 
 60 |         sensorFusion = new SensorFusion();
 61 |         sensorFusion.setMode(SensorFusion.Mode.ACC_MAG);
 62 | 
 63 |         bubbleLevelCompass = (BubbleLevelCompass) this.findViewById(R.id.SensorFusionView);
 64 |         setModeRadioGroup = (RadioGroup) findViewById(R.id.radioGroup1);
 65 |         azimuthText = (TextView) findViewById(R.id.azmuth);
 66 |         pithText = (TextView) findViewById(R.id.pitch);
 67 |         rollText = (TextView) findViewById(R.id.roll);
 68 |         setModeRadioGroup.setOnCheckedChangeListener(this);
 69 |     }
 70 | 
 71 |     public void updateOrientationDisplay() {
 72 | 
 73 |         double azimuthValue = sensorFusion.getAzimuth();
 74 |         double rollValue =  sensorFusion.getRoll();
 75 |         double pitchValue =  sensorFusion.getPitch();
 76 | 
 77 |         azimuthText.setText(String.valueOf(d.format(azimuthValue)));
 78 |         pithText.setText(String.valueOf(d.format(pitchValue)));
 79 |         rollText.setText(String.valueOf(d.format(rollValue)));
 80 | 
 81 |         bubbleLevelCompass.setPLeft((int) rollValue);
 82 |         bubbleLevelCompass.setPTop((int) pitchValue);
 83 |         bubbleLevelCompass.setAzimuth((int) azimuthValue);
 84 | 
 85 |     }
 86 | 
 87 |     public void registerSensorManagerListeners() {
 88 |         sensorManager.registerListener(this,
 89 |                 sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),
 90 |                 SensorManager.SENSOR_DELAY_FASTEST);
 91 | 
 92 |         sensorManager.registerListener(this,
 93 |                 sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE),
 94 |                 SensorManager.SENSOR_DELAY_FASTEST);
 95 | 
 96 |         sensorManager.registerListener(this,
 97 |                 sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),
 98 |                 SensorManager.SENSOR_DELAY_FASTEST);
 99 |     }
100 | 
101 |     @Override
102 |     public void onStop() {
103 |         super.onStop();
104 |         sensorManager.unregisterListener(this);
105 |     }
106 | 
107 |     @Override
108 |     protected void onPause() {
109 |         super.onPause();
110 |         sensorManager.unregisterListener(this);
111 |     }
112 | 
113 |     @Override
114 |     public void onResume() {
115 |         super.onResume();
116 |         registerSensorManagerListeners();
117 |     }
118 | 
119 |     @Override
120 |     public void onAccuracyChanged(Sensor sensor, int accuracy) {
121 |     }
122 | 
123 |     @Override
124 |     public void onSensorChanged(SensorEvent event) {
125 |         switch (event.sensor.getType()) {
126 |             case Sensor.TYPE_ACCELEROMETER:
127 |                 sensorFusion.setAccel(event.values);
128 |                 sensorFusion.calculateAccMagOrientation();
129 |                 break;
130 | 
131 |             case Sensor.TYPE_GYROSCOPE:
132 |                 sensorFusion.gyroFunction(event);
133 |                 break;
134 | 
135 |             case Sensor.TYPE_MAGNETIC_FIELD:
136 |                 sensorFusion.setMagnet(event.values);
137 |                 break;
138 |         }
139 |         updateOrientationDisplay();
140 |     }
141 | 
142 |     @Override
143 |     public void onCheckedChanged(RadioGroup group, int checkedId) {
144 | 
145 |         switch (checkedId) {
146 |             case R.id.radio0:
147 |                 sensorFusion.setMode(SensorFusion.Mode.ACC_MAG);
148 |                 break;
149 |             case R.id.radio1:
150 |                 sensorFusion.setMode(SensorFusion.Mode.GYRO);
151 |                 break;
152 |             case R.id.radio2:
153 |                 sensorFusion.setMode(SensorFusion.Mode.FUSION);
154 |                 break;
155 |         }
156 |     }
157 | 
158 | }


--------------------------------------------------------------------------------
/java/com/goatstone/util/SensorFusion.java:
--------------------------------------------------------------------------------
  1 | /************************************************************************************
  2 |  * Copyright (c) 2012 Paul Lawitzki
  3 |  * Copyright (c) 2014 Jose Collas : Wrapper code
  4 |  *
  5 |  * Permission is hereby granted, free of charge, to any person obtaining
  6 |  * a copy of this software and associated documentation files (the "Software"),
  7 |  * to deal in the Software without restriction, including without limitation
  8 |  * the rights to use, copy, modify, merge, publish, distribute, sublicense,
  9 |  * and/or sell copies of the Software, and to permit persons to whom the
 10 |  * Software is furnished to do so, subject to the following conditions:
 11 |  *
 12 |  *
 13 |  * The above copyright notice and this permission notice shall be included
 14 |  * in all copies or substantial portions of the Software.
 15 |  *
 16 |  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 17 |  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 18 |  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 19 |  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 20 |  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 21 |  * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE
 22 |  * OR OTHER DEALINGS IN THE SOFTWARE.
 23 |  ************************************************************************************/
 24 | 
 25 | package com.goatstone.util;
 26 | 
 27 | import android.hardware.SensorEvent;
 28 | import android.hardware.SensorManager;
 29 | import android.util.Log;
 30 | 
 31 | import java.util.Timer;
 32 | import java.util.TimerTask;
 33 | 
 34 | /**
 35 |  * Provides a wrapper around code that fuses sensor data.
 36 |  * @author 2012 Paul Lawitzki
 37 |  * @author 2014 Jose Collas
 38 |  *
 39 |  * @version 0.7 1/26/14.
 40 |  *
 41 |  */
 42 | public class SensorFusion {
 43 | 
 44 |     // angular speeds from gyro
 45 |     private float[] gyro = new float[3];
 46 | 
 47 |     // rotation matrix from gyro data
 48 |     private float[] gyroMatrix = new float[9];
 49 | 
 50 |     // orientation angles from gyro matrix
 51 |     private float[] gyroOrientation = new float[3];
 52 | 
 53 |     // magnetic field vector
 54 |     private float[] magnet = new float[3];
 55 | 
 56 |     // accelerometer vector
 57 |     private float[] accel = new float[3];
 58 | 
 59 |     // orientation angles from accel and magnet
 60 |     private float[] accMagOrientation = new float[3];
 61 | 
 62 |     // final orientation angles from sensor fusion
 63 |     private float[] fusedOrientation = new float[3];
 64 | 
 65 |     private float[] selectedOrientation = fusedOrientation;
 66 | 
 67 |     public enum Mode {
 68 |         ACC_MAG, GYRO, FUSION
 69 |     }
 70 | 
 71 |     // accelerometer and magnetometer based rotation matrix
 72 |     private float[] rotationMatrix = new float[9];
 73 | 
 74 |     public static final float EPSILON = 0.000000001f;
 75 |     private static final float NS2S = 1.0f / 1000000000.0f;
 76 |     private long timestamp;
 77 |     private boolean initState = true;
 78 | 
 79 |     public static final int TIME_CONSTANT = 30;
 80 |     public static final float FILTER_COEFFICIENT = 0.98f;
 81 |     private Timer fuseTimer = new Timer();
 82 | 
 83 |     public SensorFusion() {
 84 | 
 85 |         gyroOrientation[0] = 0.0f;
 86 |         gyroOrientation[1] = 0.0f;
 87 |         gyroOrientation[2] = 0.0f;
 88 | 
 89 |         // initialise gyroMatrix with identity matrix
 90 |         gyroMatrix[0] = 1.0f;
 91 |         gyroMatrix[1] = 0.0f;
 92 |         gyroMatrix[2] = 0.0f;
 93 |         gyroMatrix[3] = 0.0f;
 94 |         gyroMatrix[4] = 1.0f;
 95 |         gyroMatrix[5] = 0.0f;
 96 |         gyroMatrix[6] = 0.0f;
 97 |         gyroMatrix[7] = 0.0f;
 98 |         gyroMatrix[8] = 1.0f;
 99 | 
100 |         // wait for one second until gyroscope and magnetometer/accelerometer
101 |         // data is initialised then scedule the complementary filter task
102 |         fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(),
103 |                 1000, TIME_CONSTANT);
104 | 
105 |     }
106 | 
107 |     public double getAzimuth() {
108 |         return selectedOrientation[0] * 180 / Math.PI;
109 |     }
110 | 
111 |     public double getPitch() {
112 |         return selectedOrientation[1] * 180 / Math.PI;
113 |     }
114 | 
115 |     public double getRoll() {
116 |         return selectedOrientation[2] * 180 / Math.PI;
117 |     }
118 | 
119 |     public void setMagnet(float[] sensorValues) {
120 |         System.arraycopy(sensorValues, 0, magnet, 0, 3);
121 |     }
122 | 
123 |     public void setAccel(float[] sensorValues) {
124 |         System.arraycopy(sensorValues, 0, accel, 0, 3);
125 | 
126 |     }
127 | 
128 |     public  void setMode(Mode mode) {
129 |         Log.i("tag", "msg 0"+ mode);
130 |         Log.i("tag", "msg 00000"+ Mode.ACC_MAG);
131 |         switch (mode) {
132 |             case ACC_MAG:
133 |                 Log.i("tag", "msg 1"+ mode);
134 |                 selectedOrientation = accMagOrientation;
135 |                 break;
136 |             case GYRO:
137 |                 Log.i("tag", "msg 2"+ mode);
138 |                 selectedOrientation = gyroOrientation;
139 |                 break;
140 |             case FUSION:
141 |                 Log.i("tag", "msg 3"+ mode);
142 |                 selectedOrientation = fusedOrientation;
143 |                 break;
144 |             default:
145 |                 Log.i("tag", "msg 4"+ mode);
146 |                 selectedOrientation = fusedOrientation;
147 |                 break;
148 |         }
149 |     }
150 | 
151 |     // calculates orientation angles from accelerometer and magnetometer output
152 |     public void calculateAccMagOrientation() {
153 |         if (SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) {
154 |             SensorManager.getOrientation(rotationMatrix, accMagOrientation);
155 |         }
156 |     }
157 | 
158 |     // This function is borrowed from the Android reference
159 |     // at http://developer.android.com/reference/android/hardware/SensorEvent.html#values
160 |     // It calculates a rotation vector from the gyroscope angular speed values.
161 |     private void getRotationVectorFromGyro(float[] gyroValues,
162 |                                            float[] deltaRotationVector,
163 |                                            float timeFactor) {
164 |         float[] normValues = new float[3];
165 | 
166 |         // Calculate the angular speed of the sample
167 |         float omegaMagnitude =
168 |                 (float) Math.sqrt(gyroValues[0] * gyroValues[0] +
169 |                         gyroValues[1] * gyroValues[1] +
170 |                         gyroValues[2] * gyroValues[2]);
171 | 
172 |         // Normalize the rotation vector if it's big enough to get the axis
173 |         if (omegaMagnitude > EPSILON) {
174 |             normValues[0] = gyroValues[0] / omegaMagnitude;
175 |             normValues[1] = gyroValues[1] / omegaMagnitude;
176 |             normValues[2] = gyroValues[2] / omegaMagnitude;
177 |         }
178 | 
179 |         // Integrate around this axis with the angular speed by the timestep
180 |         // in order to get a delta rotation from this sample over the timestep
181 |         // We will convert this axis-angle representation of the delta rotation
182 |         // into a quaternion before turning it into the rotation matrix.
183 |         float thetaOverTwo = omegaMagnitude * timeFactor;
184 |         float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
185 |         float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
186 |         deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
187 |         deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
188 |         deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
189 |         deltaRotationVector[3] = cosThetaOverTwo;
190 |     }
191 | 
192 |     // This function performs the integration of the gyroscope data.
193 |     // It writes the gyroscope based orientation into gyroOrientation.
194 |     public void gyroFunction(SensorEvent event) {
195 |         // don't start until first accelerometer/magnetometer orientation has been acquired
196 |         if (accMagOrientation == null)
197 |             return;
198 | 
199 |         // initialisation of the gyroscope based rotation matrix
200 |         if (initState) {
201 |             float[] initMatrix = new float[9];
202 |             initMatrix = getRotationMatrixFromOrientation(accMagOrientation);
203 |             float[] test = new float[3];
204 |             SensorManager.getOrientation(initMatrix, test);
205 |             gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix);
206 |             initState = false;
207 |         }
208 | 
209 |         // copy the new gyro values into the gyro array
210 |         // convert the raw gyro data into a rotation vector
211 |         float[] deltaVector = new float[4];
212 |         if (timestamp != 0) {
213 |             final float dT = (event.timestamp - timestamp) * NS2S;
214 |             System.arraycopy(event.values, 0, gyro, 0, 3);
215 |             getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
216 |         }
217 | 
218 |         // measurement done, save current time for next interval
219 |         timestamp = event.timestamp;
220 | 
221 |         // convert rotation vector into rotation matrix
222 |         float[] deltaMatrix = new float[9];
223 |         SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);
224 | 
225 |         // apply the new rotation interval on the gyroscope based rotation matrix
226 |         gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);
227 | 
228 |         // get the gyroscope based orientation from the rotation matrix
229 |         SensorManager.getOrientation(gyroMatrix, gyroOrientation);
230 |     }
231 | 
232 |     private float[] getRotationMatrixFromOrientation(float[] o) {
233 |         float[] xM = new float[9];
234 |         float[] yM = new float[9];
235 |         float[] zM = new float[9];
236 | 
237 |         float sinX = (float) Math.sin(o[1]);
238 |         float cosX = (float) Math.cos(o[1]);
239 |         float sinY = (float) Math.sin(o[2]);
240 |         float cosY = (float) Math.cos(o[2]);
241 |         float sinZ = (float) Math.sin(o[0]);
242 |         float cosZ = (float) Math.cos(o[0]);
243 | 
244 |         // rotation about x-axis (pitch)
245 |         xM[0] = 1.0f;
246 |         xM[1] = 0.0f;
247 |         xM[2] = 0.0f;
248 |         xM[3] = 0.0f;
249 |         xM[4] = cosX;
250 |         xM[5] = sinX;
251 |         xM[6] = 0.0f;
252 |         xM[7] = -sinX;
253 |         xM[8] = cosX;
254 | 
255 |         // rotation about y-axis (roll)
256 |         yM[0] = cosY;
257 |         yM[1] = 0.0f;
258 |         yM[2] = sinY;
259 |         yM[3] = 0.0f;
260 |         yM[4] = 1.0f;
261 |         yM[5] = 0.0f;
262 |         yM[6] = -sinY;
263 |         yM[7] = 0.0f;
264 |         yM[8] = cosY;
265 | 
266 |         // rotation about z-axis (azimuth)
267 |         zM[0] = cosZ;
268 |         zM[1] = sinZ;
269 |         zM[2] = 0.0f;
270 |         zM[3] = -sinZ;
271 |         zM[4] = cosZ;
272 |         zM[5] = 0.0f;
273 |         zM[6] = 0.0f;
274 |         zM[7] = 0.0f;
275 |         zM[8] = 1.0f;
276 | 
277 |         // rotation order is y, x, z (roll, pitch, azimuth)
278 |         float[] resultMatrix = matrixMultiplication(xM, yM);
279 |         resultMatrix = matrixMultiplication(zM, resultMatrix);
280 |         return resultMatrix;
281 |     }
282 | 
283 |     private float[] matrixMultiplication(float[] A, float[] B) {
284 |         float[] result = new float[9];
285 | 
286 |         result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
287 |         result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
288 |         result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
289 | 
290 |         result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
291 |         result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
292 |         result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];
293 | 
294 |         result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
295 |         result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
296 |         result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];
297 | 
298 |         return result;
299 |     }
300 | 
301 |     class calculateFusedOrientationTask extends TimerTask {
302 |         public void run() {
303 |             float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT;
304 | 
305 |             /*
306 |              * Fix for 179∞ <--> -179∞ transition problem:
307 |              * Check whether one of the two orientation angles (gyro or accMag) is negative while the other one is positive.
308 |              * If so, add 360∞ (2 * math.PI) to the negative value, perform the sensor fusion, and remove the 360∞ from the result
309 |              * if it is greater than 180∞. This stabilizes the output in positive-to-negative-transition cases.
310 |              */
311 | 
312 |             // azimuth
313 |             if (gyroOrientation[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) {
314 |                 fusedOrientation[0] = (float) (FILTER_COEFFICIENT * (gyroOrientation[0] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[0]);
315 |                 fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;
316 |             } else if (accMagOrientation[0] < -0.5 * Math.PI && gyroOrientation[0] > 0.0) {
317 |                 fusedOrientation[0] = (float) (FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * (accMagOrientation[0] + 2.0 * Math.PI));
318 |                 fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;
319 |             } else {
320 |                 fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0];
321 |             }
322 | 
323 |             // pitch
324 |             if (gyroOrientation[1] < -0.5 * Math.PI && accMagOrientation[1] > 0.0) {
325 |                 fusedOrientation[1] = (float) (FILTER_COEFFICIENT * (gyroOrientation[1] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[1]);
326 |                 fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0;
327 |             } else if (accMagOrientation[1] < -0.5 * Math.PI && gyroOrientation[1] > 0.0) {
328 |                 fusedOrientation[1] = (float) (FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * (accMagOrientation[1] + 2.0 * Math.PI));
329 |                 fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0;
330 |             } else {
331 |                 fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * accMagOrientation[1];
332 |             }
333 | 
334 |             // roll
335 |             if (gyroOrientation[2] < -0.5 * Math.PI && accMagOrientation[2] > 0.0) {
336 |                 fusedOrientation[2] = (float) (FILTER_COEFFICIENT * (gyroOrientation[2] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[2]);
337 |                 fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0;
338 |             } else if (accMagOrientation[2] < -0.5 * Math.PI && gyroOrientation[2] > 0.0) {
339 |                 fusedOrientation[2] = (float) (FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * (accMagOrientation[2] + 2.0 * Math.PI));
340 |                 fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0;
341 |             } else {
342 |                 fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * accMagOrientation[2];
343 |             }
344 | 
345 |             // overwrite gyro matrix and orientation with fused orientation
346 |             // to comensate gyro drift
347 |             gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
348 |             System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
349 | 
350 | 
351 |             // update sensor output in GUI
352 |             //mainHandler.post(updateOreintationDisplayTask);
353 |         }
354 |     }
355 | 
356 | 
357 | }
358 | 


--------------------------------------------------------------------------------
/res/drawable-ldpi/ic_launcher.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/goatstone/AndroidSensorFusion/2fc70e56c24efc229cd4d5327d8e65b2bb6e6753/res/drawable-ldpi/ic_launcher.png


--------------------------------------------------------------------------------
/res/drawable-mdpi/ic_launcher.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/goatstone/AndroidSensorFusion/2fc70e56c24efc229cd4d5327d8e65b2bb6e6753/res/drawable-mdpi/ic_launcher.png


--------------------------------------------------------------------------------
/res/ic_launcher/ic_launcher.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/goatstone/AndroidSensorFusion/2fc70e56c24efc229cd4d5327d8e65b2bb6e6753/res/ic_launcher/ic_launcher.png


--------------------------------------------------------------------------------
/res/layout/main.xml:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="utf-8"?>
  2 | <LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
  3 |     android:layout_width="fill_parent"
  4 |     android:layout_height="fill_parent"
  5 |     android:orientation="vertical" >
  6 | 
  7 |     <TableLayout
  8 |         android:id="@+id/tableLayout1"
  9 |         android:layout_width="match_parent"
 10 |         android:layout_height="wrap_content" >
 11 | 
 12 |         <TableRow
 13 |             android:id="@+id/tableRow1"
 14 |             android:layout_width="match_parent"
 15 |             android:layout_height="wrap_content"
 16 |             android:layout_gravity="right"
 17 |             android:layout_weight="1" >
 18 | 
 19 |             <TextView
 20 |                 android:id="@+id/textView1"
 21 |                 android:layout_width="wrap_content"
 22 |                 android:layout_height="wrap_content"
 23 |                 android:textSize="20dp"
 24 |                 android:text="Azimuth (z-axis rotation): " />
 25 | 
 26 |             <TextView
 27 |                 android:id="@+id/azmuth"
 28 |                 android:layout_width="wrap_content"
 29 |                 android:layout_height="wrap_content"
 30 |                 android:layout_marginLeft="10px"
 31 |                 android:text="0000"
 32 |                 android:textSize="20dp"
 33 |                 android:textAppearance="?android:attr/textAppearanceMedium" />
 34 | 
 35 |         </TableRow>
 36 | 
 37 | 
 38 |         <TableRow
 39 |             android:id="@+id/tableRow2"
 40 |             android:layout_width="wrap_content"
 41 |             android:layout_height="wrap_content"
 42 |             android:layout_gravity="right" >
 43 | 
 44 |             <TextView
 45 |                 android:id="@+id/textView2"
 46 |                 android:layout_width="wrap_content"
 47 |                 android:layout_height="wrap_content"
 48 |                 android:textSize="20dp"
 49 |                 android:text="Pitch (x-axis rotation): " />
 50 | 
 51 |             <TextView
 52 |                 android:id="@+id/pitch"
 53 |                 android:layout_width="wrap_content"
 54 |                 android:layout_height="wrap_content"
 55 |                 android:layout_marginLeft="10px"
 56 |                 android:text="ppp"
 57 |                 android:textSize="20dp"
 58 |                 android:textAppearance="?android:attr/textAppearanceMedium" />
 59 |         </TableRow>
 60 | 
 61 | 
 62 | 
 63 | 
 64 |         <TableRow
 65 |             android:id="@+id/tableRow3"
 66 |             android:layout_width="wrap_content"
 67 |             android:layout_height="wrap_content"
 68 |             android:layout_gravity="right"
 69 |             android:baselineAligned="true" >
 70 | 
 71 |             <TextView
 72 |                 android:id="@+id/textView3"
 73 |                 android:layout_width="wrap_content"
 74 |                 android:layout_height="wrap_content"
 75 |                 android:textSize="20dp"
 76 |                 android:text="Roll (y-axis ritation): " />
 77 | 
 78 |             <TextView
 79 |                 android:id="@+id/roll"
 80 |                 android:layout_width="wrap_content"
 81 |                 android:layout_height="wrap_content"
 82 |                 android:layout_marginLeft="10px"
 83 |                 android:text="rrr"
 84 |                 android:textSize="20dp"
 85 |                 android:textAppearance="?android:attr/textAppearanceMedium" />
 86 | 
 87 |         </TableRow>
 88 | 
 89 |         <TableRow
 90 |             android:id="@+id/tableRow4"
 91 |             android:layout_width="wrap_content"
 92 |             android:layout_height="wrap_content" >
 93 | 
 94 |         </TableRow>
 95 |     </TableLayout>
 96 | 
 97 |     <RadioGroup
 98 |         android:id="@+id/radioGroup1"
 99 |         android:layout_width="wrap_content"
100 |         android:layout_height="wrap_content" >
101 | 
102 |         <RadioButton
103 |             android:id="@+id/radio0"
104 |             android:layout_width="wrap_content"
105 |             android:layout_height="wrap_content"
106 |             android:checked="true"
107 |             android:textSize="20dp"
108 |             android:text="Accelerometer/Magnetometer Orientation" />
109 | 
110 |         <RadioButton
111 |             android:id="@+id/radio1"
112 |             android:layout_width="wrap_content"
113 |             android:layout_height="wrap_content"
114 |             android:textSize="20dp"
115 |             android:text="Gyroscope Orientation (drift eliminated)" />
116 | 
117 |         <RadioButton
118 |             android:id="@+id/radio2"
119 |             android:layout_width="wrap_content"
120 |             android:layout_height="wrap_content"
121 |             android:textSize="20dp"
122 |             android:text="Orientation from Sensor Fusion" />
123 |     </RadioGroup>
124 | 
125 |     <com.goatstone.sensorFusion.BubbleLevelCompass
126 |         android:id="@+id/SensorFusionView"
127 |         android:layout_width="800dp"
128 |         android:layout_height="800dp"
129 |         android:padding="0dp"
130 |         android:layout_weight="100"
131 |         android:background="@android:color/white"
132 |         android:layout_gravity="center_horizontal"
133 |   />
134 | 
135 | </LinearLayout>


--------------------------------------------------------------------------------
/res/values/strings.xml:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="utf-8"?>
2 | <resources>
3 | 
4 |     <string name="hello">Hello</string>
5 |     <string name="app_name">SensorFusion</string>
6 | 
7 | </resources>


--------------------------------------------------------------------------------