陀螺仪源码 android

发表于 2015-12-03 07:47:55 by 月小升

package com.toplus.speedtest;
 
import java.util.Locale;
 
import com.toplus.speedtest.R;
 
import android.app.ActionBar;
import android.app.Activity;
import android.app.FragmentTransaction;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.support.v4.app.Fragment;
import android.support.v4.app.FragmentActivity;
import android.support.v4.app.FragmentManager;
import android.support.v4.app.FragmentPagerAdapter;
import android.support.v4.app.NavUtils;
import android.support.v4.view.ViewPager;
import android.view.Gravity;
import android.view.LayoutInflater;
import android.view.Menu;
import android.view.MenuItem;
import android.view.View;
import android.view.ViewGroup;
import android.widget.TextView;
import android.widget.Toast;
 
 
public class MainActivity extends Activity implements SensorEventListener
{
 
private SensorManager sm;
private Sensor mGyroscope;
private Sensor mAccelerator;
private Sensor GAccelerator;
private TextView axis_x = null ,axis_y = null ,axis_z = null;
private TextView axis_x_degree = null, axis_y_degree = null, axis_z_degree = null ;
private TextView G_axis_x = null ,G_axis_y = null ,G_axis_z = null;
private TextView axis_gro_x_degree = null;// ,G_axis_y = null ,G_axis_z = null;
private double X_max=0, Y_max=0, Z_max=0;
public static final double G_CONSTANT = 9.81;
private double degree_X = 0 , offset_X = 0 ;
private double degree_Y = 0 , offset_Y = 0 ;
private double degree_Z = 0 , offset_Z = 0 ;
private long lasttimestamp=0;
private TextView R_total = null ;
private static final double C= 180 / Math.PI / 1000000000;
private static final float NS2S = 1.0f / 1000000000.0f;
private final float[] deltaRotationVector = new float[4];
private float timestamp;
privatestatic float P[][] = {
{
 1, 0
}, {
 0, 1
}
};
privatefloat angle=0;
privatefloat q_bias=0;
privatefloat rate=0;
privatestatic float R_angle = (float) 0.5 ;
privatestatic float Q_angle = (float) 0.001;
privatestatic float Q_gyro  = (float) 0.003;
private double gro_degree_X = 0;
float gx = 0;
    float gy = 0;
    float gz = 0;
    float Lax=0, Lay=0,Laz=0;
    float M_ax=0, M_ay=0,M_az=0;
    double rad_x=0,cos_x=0;
    double rad_y=0,cos_y=0;
    double rad_z=0,cos_z=0;
    float dT;
    double Rt=0,EPSILON=0.1,VS=0;
 
@Override
protected void onCreate(Bundle savedInstanceState)
{
 
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_main);
sm = (SensorManager)getSystemService(SENSOR_SERVICE);
 
//List sensors = sm.getSensorList(Sensor.TYPE_GYROSCOPE);
mGyroscope = sm.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
if(mGyroscope==null){
 
Toast.makeText(this, "您的设备不支持陀螺仪！", 1).show();
 
}else{
 
 
/**
 * 注册监听器
 */
sm.registerListener( this , mGyroscope , SensorManager.SENSOR_DELAY_NORMAL);
 
}
mAccelerator = sm.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
GAccelerator = sm.getDefaultSensor(Sensor.TYPE_GRAVITY);
if(GAccelerator==null){
 
Toast.makeText(this, "您的设备不支持重力计！", 1).show();
 
}else{
 
 
/**
 * 注册监听器
 */
sm.registerListener( this , GAccelerator , SensorManager.SENSOR_DELAY_NORMAL);
 
}
//sm.registerListener( this , GAccelerator , SensorManager.SENSOR_DELAY_NORMAL);
sm.registerListener( this , mAccelerator , SensorManager.SENSOR_DELAY_UI);
 
 
}
@Override
protected void onResume(){
 
super.onResume();
lasttimestamp = 0;
degree_X = 0 ;
degree_Y = 0 ;
degree_Z = 0 ;
 
}
 
@Override
public boolean onCreateOptionsMenu(Menu menu)
{
 
// Inflate the menu; this adds items to the action bar if it is present.
//getMenuInflater().inflate(R.menu.activity_main, menu);
return true;
 
}
 
public void onAccuracyChanged(Sensor sensor, int accuracy) {
 
}
public void onSensorChanged(SensorEvent event) {
 
if( axis_x == null ) axis_x = (TextView)findViewById(R.id.axis_x);
if( axis_y == null ) axis_y = (TextView)findViewById(R.id.axis_y);
if( axis_z == null ) axis_z = (TextView)findViewById(R.id.axis_z);
if( axis_x_degree == null )axis_x_degree = (TextView)findViewById(R.id.axis_x_degree);
if( axis_y_degree == null )axis_y_degree = (TextView)findViewById(R.id.axis_y_degree);
if( axis_z_degree == null )axis_z_degree = (TextView)findViewById(R.id.axis_z_degree);
if( G_axis_x == null ) G_axis_x = (TextView)findViewById(R.id.G_axis_x);
if( G_axis_y == null ) G_axis_y = (TextView)findViewById(R.id.G_axis_y);
if( G_axis_z == null ) G_axis_z = (TextView)findViewById(R.id.G_axis_z);
if( R_total == null ) R_total = (TextView)findViewById(R.id.R_total);
if( axis_gro_x_degree == null ) axis_gro_x_degree = (TextView)findViewById(R.id.axis_gro_x_degree);
 
 
if( lasttimestamp == 0){
 
lasttimestamp = event.timestamp ;
return;
 
}
 
 
if( event.sensor == mGyroscope ){
 
if( abs(event.values[0]) > abs(X_max)){
 
X_max = event.values[0];
 
}
 
if( abs(event.values[1]) > abs(Y_max)){
 
Y_max = event.values[1];
 
}
 
if( abs(event.values[2]) > abs(Z_max)){
 
Z_max = event.values[2];
 
}
if (timestamp != 0) {
 
dT = (event.timestamp - timestamp) * NS2S;
float axisX = event.values[0];
    float axisY = event.values[1];
    float axisZ = event.values[2];
     
     
 
   // timestamp = event.timestamp;   
    
axis_x.setText("X="+String.valueOf(axisX));//+" ("+X_max+")");
axis_y.setText("Y="+String.valueOf(axisY));//+" ("+Y_max+")");
axis_z.setText("Z="+String.valueOf(axisZ));//+" ("+Z_max+")");
 
// 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
    // (that is, EPSILON should represent your maximum allowable margin of error)
    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;
// 
}
 
//float[] deltaRotationMatrix = new float[9];
//  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;
 
//gro_degree_X=(180/Math.PI)*angle;
     
 degree_X += axisX * dT;
 degree_Y += axisY * dT;
 degree_Z += axisZ * dT;
  
//degree_X = 180*deltaRotationVector[0]/Math.PI;
     
}
 
}
    timestamp = event.timestamp; 
     
 
 
axis_x_degree.setText("degree="+String.valueOf(180*degree_X/Math.PI));//+" ("+X_max+")");
//axis_gro_x_degree.setText("degree="+String.valueOf(degree_X));//+" ("+X_max+")");
axis_y_degree.setText("degree="+String.valueOf(180*degree_Y/Math.PI));//+" ("+X_max+")");
axis_z_degree.setText("degree="+String.valueOf(180*degree_Z/Math.PI));//+" ("+X_max+")");
 
     
 
}else if(event.sensor ==mAccelerator) {
 
 
 float[] values = event.values;
        M_ax = values[0];
        M_ay = values[1];
        M_az = values[2];
         
         
 
}else if(event.sensor ==GAccelerator){
 
 
 
        float[] values = event.values;
        Lax = values[0];
        Lay = values[1];
        Laz = values[2];
 
        final float alpha = (float) 0.8;
   //     gravity[0] = alpha * gravity[0] + (1 - alpha) * M_ax;
   //     gravity[1] = alpha * gravity[1] + (1 - alpha) * M_ay;
   //     gravity[2] = alpha * gravity[2] + (1 - alpha) * M_az;
   //     Lax = M_ax - gravity[0];
   //     Lay = M_ay - gravity[1];
   //     Laz = M_az - gravity[2];
         
        gx=M_ax-Lax;
        gy=M_ay-Lay;
        gz=M_az-Laz;
     //Rt=Math.sqrt(Lax*Lax+Lay*Lay+Laz*Laz);
      
       if(Laz>0){
      
        Rt=Math.sqrt(Lax*Lax+Lay*Lay+Laz*Laz);
        
}else{
 
      Rt=-Math.sqrt(Lax*Lax+Lay*Lay+Laz*Laz);
        
}
         
      VS += Rt*dT;
       
      //timestamp = event.timestamp;
       
 
}
 
 
G_axis_x.setText("X="+String.valueOf(Lax)+"   "+ String.valueOf(M_ax));
G_axis_y.setText("Y="+String.valueOf(Lay)+"   "+ String.valueOf(M_ay));
G_axis_z.setText("Z="+String.valueOf(Laz)+"   "+ String.valueOf(M_az));
R_total.setText("R="+String.valueOf(VS));
 
 
 
 
}
 
 
private double abs(double val){
 
if(val < 0) return -val;
return val;
 
}
 
}

---

首发地址：月小升博客 – https://java-er.com/blog/tly-android/
无特殊说明，文章均为月小升原创，欢迎转载，转载请注明本文地址，谢谢