发表于 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);

//Listsensors = 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;

}

}

This entry was posted in JAVA and tagged Android, 陀螺仪. Bookmark the permalink.

月小升QQ 2651044202, 技术交流QQ群 178491360

首发地址：月小升博客 – https://java-er.com/blog/tly-android/

无特殊说明，文章均为月小升原创，欢迎转载，转载请注明本文地址，谢谢