陀螺仪源码 android


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;

}

}
如果你是一名技术人员可加我QQ 2651-0442-02,如果你是java技术人还可以加入QQ群 1784-9136-0
你将得到的不仅仅是技术的交流,还有职业机会,人生解惑.
首发地址:月小升博客https://java-er.com/blog/tly-android/
无特殊说明,文章均为月小升原创,欢迎转载,转载请注明本文地址,谢谢
此条目发表在 JAVA 分类目录,贴了 , 标签。将固定链接加入收藏夹。
既然来了,就评论一下,不会怀孕的

发表评论

电子邮件地址不会被公开。 必填项已用 * 标注

*

您可以使用这些 HTML 标签和属性: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong> <pre lang="" line="" escaped="" highlight="">