Uso de giroscopio Android en lugar de acelerómetro. Encontré un montón de fragmentos, pero no hay código completo

El video Sensor Fusion se ve genial, pero no hay código: http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=player_detailpage#t=1315s

Aquí está mi código que sólo utiliza acelerómetro y brújula. También uso un filtro de Kalman en los 3 valores de orientación, pero eso es demasiado código para mostrar aquí. En última instancia, esto funciona bien, pero el resultado es demasiado nervioso o demasiado laggy dependiendo de lo que hago con los resultados y lo bajo que hago los factores de filtración.

• ¿Qué significa este Android Lint: "Orientación errónea?"
• Manera correcta de usar SOLO Giroscopio y Acelerómetro para obtener el ángulo de corriente fiable en cualquier eje de ANDROID
• Cómo habilitar sólo retratos y retratos en los teléfonos, "sensorPortrait" no funciona en los teléfonos?
• Orientación de vídeo de retrato de Android incorrecta en VideoView
• ¿Cómo debo calcular el azimut, el tono, la orientación cuando mi dispositivo Android no es plano?

/** Just accelerometer and magnetic sensors */ public abstract class SensorsListener2 implements SensorEventListener { /** The lower this is, the greater the preference which is given to previous values. (slows change) */ private static final float accelFilteringFactor = 0.1f; private static final float magFilteringFactor = 0.01f; public abstract boolean getIsLandscape(); @Override public void onSensorChanged(SensorEvent event) { Sensor sensor = event.sensor; int type = sensor.getType(); switch (type) { case Sensor.TYPE_MAGNETIC_FIELD: mags[0] = event.values[0] * magFilteringFactor + mags[0] * (1.0f - magFilteringFactor); mags[1] = event.values[1] * magFilteringFactor + mags[1] * (1.0f - magFilteringFactor); mags[2] = event.values[2] * magFilteringFactor + mags[2] * (1.0f - magFilteringFactor); isReady = true; break; case Sensor.TYPE_ACCELEROMETER: accels[0] = event.values[0] * accelFilteringFactor + accels[0] * (1.0f - accelFilteringFactor); accels[1] = event.values[1] * accelFilteringFactor + accels[1] * (1.0f - accelFilteringFactor); accels[2] = event.values[2] * accelFilteringFactor + accels[2] * (1.0f - accelFilteringFactor); break; default: return; } if(mags != null && accels != null && isReady) { isReady = false; SensorManager.getRotationMatrix(rot, inclination, accels, mags); boolean isLandscape = getIsLandscape(); if(isLandscape) { outR = rot; } else { // Remap the coordinates to work in portrait mode. SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR); } SensorManager.getOrientation(outR, values); double x180pi = 180.0 / Math.PI; float azimuth = (float)(values[0] * x180pi); float pitch = (float)(values[1] * x180pi); float roll = (float)(values[2] * x180pi); // In landscape mode swap pitch and roll and invert the pitch. if(isLandscape) { float tmp = pitch; pitch = -roll; roll = -tmp; azimuth = 180 - azimuth; } else { pitch = -pitch - 90; azimuth = 90 - azimuth; } onOrientationChanged(azimuth,pitch,roll); } } private float[] mags = new float[3]; private float[] accels = new float[3]; private boolean isReady; private float[] rot = new float[9]; private float[] outR = new float[9]; private float[] inclination = new float[9]; private float[] values = new float[3]; /** Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis. Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis. */ public abstract void onOrientationChanged(float azimuth, float pitch, float roll); } 

Intenté averiguar cómo agregar datos de giroscopios, pero no lo hago bien. El documento de google en http://developer.android.com/reference/android/hardware/SensorEvent.html muestra algún código para obtener una matriz delta de los datos del giroscopio. La idea parece ser que me manivela los filtros para el acelerómetro y sensores magnéticos para que fueran realmente estable. Eso haría un seguimiento de la orientación a largo plazo.

Entonces, mantendría una historia de las matrices N delta más recientes del giroscopio. Cada vez que conseguía una nueva, soltaría la más antigua y las multiplicaría para obtener una matriz final que se multiplicaría contra la matriz estable devuelta por el acelerómetro y los sensores magnéticos.

Esto no parece funcionar. O, al menos, mi implementación no funciona. El resultado es mucho más nervioso que el acelerómetro. Aumentar el tamaño de la historia del giroscopio en realidad aumenta la fluctuación de fase que me hace pensar que no estoy calculando los valores correctos del giroscopio.

 public abstract class SensorsListener3 implements SensorEventListener { /** The lower this is, the greater the preference which is given to previous values. (slows change) */ private static final float kFilteringFactor = 0.001f; private static final float magKFilteringFactor = 0.001f; public abstract boolean getIsLandscape(); @Override public void onSensorChanged(SensorEvent event) { Sensor sensor = event.sensor; int type = sensor.getType(); switch (type) { case Sensor.TYPE_MAGNETIC_FIELD: mags[0] = event.values[0] * magKFilteringFactor + mags[0] * (1.0f - magKFilteringFactor); mags[1] = event.values[1] * magKFilteringFactor + mags[1] * (1.0f - magKFilteringFactor); mags[2] = event.values[2] * magKFilteringFactor + mags[2] * (1.0f - magKFilteringFactor); isReady = true; break; case Sensor.TYPE_ACCELEROMETER: accels[0] = event.values[0] * kFilteringFactor + accels[0] * (1.0f - kFilteringFactor); accels[1] = event.values[1] * kFilteringFactor + accels[1] * (1.0f - kFilteringFactor); accels[2] = event.values[2] * kFilteringFactor + accels[2] * (1.0f - kFilteringFactor); break; case Sensor.TYPE_GYROSCOPE: gyroscopeSensorChanged(event); break; default: return; } if(mags != null && accels != null && isReady) { isReady = false; SensorManager.getRotationMatrix(rot, inclination, accels, mags); boolean isLandscape = getIsLandscape(); if(isLandscape) { outR = rot; } else { // Remap the coordinates to work in portrait mode. SensorManager.remapCoordinateSystem(rot, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR); } if(gyroUpdateTime!=0) { matrixHistory.mult(matrixTmp,matrixResult); outR = matrixResult; } SensorManager.getOrientation(outR, values); double x180pi = 180.0 / Math.PI; float azimuth = (float)(values[0] * x180pi); float pitch = (float)(values[1] * x180pi); float roll = (float)(values[2] * x180pi); // In landscape mode swap pitch and roll and invert the pitch. if(isLandscape) { float tmp = pitch; pitch = -roll; roll = -tmp; azimuth = 180 - azimuth; } else { pitch = -pitch - 90; azimuth = 90 - azimuth; } onOrientationChanged(azimuth,pitch,roll); } } private void gyroscopeSensorChanged(SensorEvent event) { // This timestep's delta rotation to be multiplied by the current rotation // after computing it from the gyro sample data. if(gyroUpdateTime != 0) { final float dT = (event.timestamp - gyroUpdateTime) * NS2S; // Axis of the rotation sample, not normalized yet. float axisX = event.values[0]; float axisY = event.values[1]; float axisZ = event.values[2]; // Calculate the angular speed of the sample float omegaMagnitude = (float)Math.sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ); // Normalize the rotation vector if it's big enough to get the axis if(omegaMagnitude > EPSILON) { axisX /= omegaMagnitude; axisY /= omegaMagnitude; axisZ /= omegaMagnitude; } // Integrate around this axis with the angular speed by the timestep // in order to get a delta rotation from this sample over the timestep // We will convert this axis-angle representation of the delta rotation // into a quaternion before turning it into the rotation matrix. float thetaOverTwo = omegaMagnitude * dT / 2.0f; float sinThetaOverTwo = (float)Math.sin(thetaOverTwo); float cosThetaOverTwo = (float)Math.cos(thetaOverTwo); deltaRotationVector[0] = sinThetaOverTwo * axisX; deltaRotationVector[1] = sinThetaOverTwo * axisY; deltaRotationVector[2] = sinThetaOverTwo * axisZ; deltaRotationVector[3] = cosThetaOverTwo; } gyroUpdateTime = event.timestamp; SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector); // User code should concatenate the delta rotation we computed with the current rotation // in order to get the updated rotation. // rotationCurrent = rotationCurrent * deltaRotationMatrix; matrixHistory.add(deltaRotationMatrix); } private float[] mags = new float[3]; private float[] accels = new float[3]; private boolean isReady; private float[] rot = new float[9]; private float[] outR = new float[9]; private float[] inclination = new float[9]; private float[] values = new float[3]; // gyroscope stuff private long gyroUpdateTime = 0; private static final float NS2S = 1.0f / 1000000000.0f; private float[] deltaRotationMatrix = new float[9]; private final float[] deltaRotationVector = new float[4]; //TODO: I have no idea how small this value should be. private static final float EPSILON = 0.000001f; private float[] matrixMult = new float[9]; private MatrixHistory matrixHistory = new MatrixHistory(100); private float[] matrixTmp = new float[9]; private float[] matrixResult = new float[9]; /** Azimuth: angle between the magnetic north direction and the Y axis, around the Z axis (0 to 359). 0=North, 90=East, 180=South, 270=West Pitch: rotation around X axis (-180 to 180), with positive values when the z-axis moves toward the y-axis. Roll: rotation around Y axis (-90 to 90), with positive values when the x-axis moves toward the z-axis. */ public abstract void onOrientationChanged(float azimuth, float pitch, float roll); } public class MatrixHistory { public MatrixHistory(int size) { vals = new float[size][]; } public void add(float[] val) { synchronized(vals) { vals[ix] = val; ix = (ix + 1) % vals.length; if(ix==0) full = true; } } public void mult(float[] tmp, float[] output) { synchronized(vals) { if(full) { for(int i=0; i<vals.length; ++i) { if(i==0) { System.arraycopy(vals[i],0,output,0,vals[i].length); } else { MathUtils.multiplyMatrix3x3(output,vals[i],tmp); System.arraycopy(tmp,0,output,0,tmp.length); } } } else { if(ix==0) return; for(int i=0; i<ix; ++i) { if(i==0) { System.arraycopy(vals[i],0,output,0,vals[i].length); } else { MathUtils.multiplyMatrix3x3(output,vals[i],tmp); System.arraycopy(tmp,0,output,0,tmp.length); } } } } } private int ix = 0; private boolean full = false; private float[][] vals; } 

El segundo bloque de código contiene mis cambios desde el primer bloque de código que agrega el giroscopio a la mezcla.

Específicamente, el factor de filtración para accel se hace más pequeño (haciendo el valor más estable). La clase MatrixHistory registra los últimos 100 valores de deltaRotationMatrix del giroscopio que se calculan en el método gyroscopeSensorChanged.

He visto muchas preguntas en este sitio sobre este tema. Me han ayudado a llegar a este punto, pero no puedo averiguar qué hacer a continuación. Realmente deseo que el tipo Sensor Fusion acabara de publicar algún código en alguna parte. Evidentemente, tenía todo preparado.

  // Requires 1.5 or above class Foo extends Activity implements SensorEventListener { SensorManager sensorManager; float[] gData = new float[3]; // Gravity or accelerometer float[] mData = new float[3]; // Magnetometer float[] orientation = new float[3]; float[] Rmat = new float[9]; float[] R2 = new float[9]; float[] Imat = new float[9]; boolean haveGrav = false; boolean haveAccel = false; boolean haveMag = false; onCreate() { // Get the sensor manager from system services sensorManager = (SensorManager)getSystemService(Context.SENSOR_SERVICE); } onResume() { super.onResume(); // Register our listeners Sensor gsensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY); Sensor asensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER); Sensor msensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD); sensorManager.registerListener(this, gsensor, SensorManager.SENSOR_DELAY_GAME); sensorManager.registerListener(this, asensor, SensorManager.SENSOR_DELAY_GAME); sensorManager.registerListener(this, msensor, SensorManager.SENSOR_DELAY_GAME); } public void onSensorChanged(SensorEvent event) { float[] data; switch( event.sensor.getType() ) { case Sensor.TYPE_GRAVITY: gData[0] = event.values[0]; gData[1] = event.values[1]; gData[2] = event.values[2]; haveGrav = true; break; case Sensor.TYPE_ACCELEROMETER: if (haveGrav) break; // don't need it, we have better gData[0] = event.values[0]; gData[1] = event.values[1]; gData[2] = event.values[2]; haveAccel = true; break; case Sensor.TYPE_MAGNETIC_FIELD: mData[0] = event.values[0]; mData[1] = event.values[1]; mData[2] = event.values[2]; haveMag = true; break; default: return; } if ((haveGrav || haveAccel) && haveMag) { SensorManager.getRotationMatrix(Rmat, Imat, gData, mData); SensorManager.remapCoordinateSystem(Rmat, SensorManager.AXIS_Y, SensorManager.AXIS_MINUS_X, R2); // Orientation isn't as useful as a rotation matrix, but // we'll show it here anyway. SensorManager.getOrientation(R2, orientation); float incl = SensorManager.getInclination(Imat); Log.d(TAG, "mh: " + (int)(orientation[0]*DEG)); Log.d(TAG, "pitch: " + (int)(orientation[1]*DEG)); Log.d(TAG, "roll: " + (int)(orientation[2]*DEG)); Log.d(TAG, "yaw: " + (int)(orientation[0]*DEG)); Log.d(TAG, "inclination: " + (int)(incl*DEG)); } } }