Example usage for android.hardware SensorManager getQuaternionFromVector

Introduction

In this page you can find the example usage for android.hardware SensorManager getQuaternionFromVector.

Prototype

public static void getQuaternionFromVector(float[] Q, float[] rv) 

Source Link

Document

Helper function to convert a rotation vector to a normalized quaternion.

Usage

From source file:eu.intermodalics.tango_ros_streamer.ImuNode.java

@Override
public void onSensorChanged(SensorEvent event) {
    switch (event.sensor.getType()) {
    case Sensor.TYPE_ROTATION_VECTOR:
        mNewRotationData = true;//ww w.j  a  v  a 2s . c  om
        float[] quaternion = new float[4];
        SensorManager.getQuaternionFromVector(quaternion, event.values);
        mImuMessage.getOrientation().setW(quaternion[0]);
        mImuMessage.getOrientation().setX(quaternion[1]);
        mImuMessage.getOrientation().setY(quaternion[2]);
        mImuMessage.getOrientation().setZ(quaternion[3]);
        mImuMessage.setOrientationCovariance(new double[] { 0, 0, 0, 0, 0, 0, 0, 0, 0 });
        break;
    case Sensor.TYPE_GYROSCOPE:
        mNewGyroscopeData = true;
        mImuMessage.getAngularVelocity().setX(event.values[0]);
        mImuMessage.getAngularVelocity().setY(event.values[1]);
        mImuMessage.getAngularVelocity().setZ(event.values[2]);
        mImuMessage.setAngularVelocityCovariance(new double[] { 0, 0, 0, 0, 0, 0, 0, 0, 0 });
        break;
    case Sensor.TYPE_ACCELEROMETER:
        mNewAccelerometerData = true;
        mImuMessage.getLinearAcceleration().setX(event.values[0]);
        mImuMessage.getLinearAcceleration().setY(event.values[1]);
        mImuMessage.getLinearAcceleration().setZ(event.values[2]);
        mImuMessage.setLinearAccelerationCovariance(new double[] { 0, 0, 0, 0, 0, 0, 0, 0, 0 });
        break;
    default:
        break;
    }
    if (mNewRotationData && mNewGyroscopeData && mNewAccelerometerData) {
        mImuMessage.getHeader().setStamp(mConnectedNode.getCurrentTime());
        mImuMessage.getHeader().setFrameId("imu");
        mImuPublisher.publish(mImuMessage);
        mNewRotationData = false;
        mNewGyroscopeData = false;
        mNewAccelerometerData = false;
    }
}

---

From source file:com.thalmic.android.sample.helloworld.HelloWorldActivity.java

public void calculateAngles(float[] result, float[] rVector, float[] accVector, float[] magVector) {
    //caculate temp rotation matrix from rotation vector first
    SensorManager.getRotationMatrix(rMatrix, null, accVector, magVector);
    SensorManager.getQuaternionFromVector(quaternion, rVector);

    roll = Math.atan2(2.0f * (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]),
            1.0f - 2.0f * (quaternion[1] * quaternion[1] + quaternion[2] * quaternion[2]));
    pitch = Math.asin(2.0f * (quaternion[0] * quaternion[2] - quaternion[3] * quaternion[1]));
    yaw = Math.atan2(2.0f * (quaternion[0] * quaternion[3] + quaternion[1] * quaternion[2]),
            1.0f - 2.0f * (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]));

    rollW = ((roll + Math.PI) / (Math.PI * 2.0) * SCALE);
    pitchW = ((pitch + Math.PI / 2.0) / Math.PI * SCALE);
    yawW = ((yaw + Math.PI) / (Math.PI * 2.0) * SCALE);

    //calculate Euler angles now
    SensorManager.getOrientation(rMatrix, result);

    //Now we can convert it to degrees
    convertToDegrees(result);//w w  w . j a v a2  s . co  m
}

---