使用磁力计的简单指南针-Android（Java）

Question

I am using Sensortag CC2541 (connected to Nexus phone)

I am using the Magnetometer of this sensor to make a simple compass. I am using the following equations to find North, South, West, and East.

Direction (y>0) = 90 - [arcTAN(x/y)]*180/pi

Direction (y<0) = 270 - [arcTAN(x/y)]*180/pi

Direction (y=0, x<0) = 180.0

Direction (y=0, x>0) = 0.0

Here's the code i'm using in my updateMagnetometer method

    @Override
    public void onUpdateMagnetometer(SensorTagManager mgr, Point3D b) {
        super.onUpdateMagnetometer(mgr, b);

        double m=0;

        if(b.x < 0.0 && (b.y < 0.01 && b.y > 0.0))
            m = 180;
        else if(b.x > 0.0 && (b.y < 0.01 && b.y > 0.0))
            m = 0;
        else if(b.y > 0.0)
            m = 90 - (Math.atan(b.x/b.y))*(180/Math.PI);
        else if(b.y < 0.0)
            m = 270 - (Math.atan(b.x/b.y))*(180/Math.PI);

        final float rot = (float) m;

        runOnUiThread(new Runnable() {

            @Override
            public void run() {
                pointer.setRotation(-rot); //pointer is the image of my compass
            }
        });
    }

But when I run it, the compass is always stuck between North and West NO MATTER how much I move, rotate, or change the position of the sensor.

Any mistakes I might be doing?

And also, I have looked a lot on the internet for making compass help. But I can't find anything. Any coding tips, pseudo code, or hint to make compass using magnetometer, accelerometer, and/or gyroscope???

---

Extra info

So this is in my main class. These methods are called every x seconds to update the readings. PS: my sensor has 2 buttons (right and left)

    mStManager.enableSensor(Sensor.MAGNETOMETER,MAGNETOMETER_UPDATE_PERIOD);
    mStManager.enableSensor(Sensor.ACCELEROMETER,MAGNETOMETER_UPDATE_PERIOD);    

    @Override
    public void onUpdateAmbientTemperature(SensorTagManager mgr, double temp) {
        super.onUpdateAmbientTemperature(mgr, temp);
    }

    @Override
    public void onUpdateAccelerometer(SensorTagManager mgr, Point3D acc) {
        super.onUpdateAccelerometer(mgr, acc);

    }

    @Override
    public void onUpdateBarometer(SensorTagManager mgr, double pressure, double height) {
        super.onUpdateBarometer(mgr, pressure, height);
    }

    @Override
    public void onUpdateGyroscope(SensorTagManager mgr, Point3D ang) {
        super.onUpdateGyroscope(mgr, ang);
    }

    @Override
    public void onUpdateHumidity(SensorTagManager mgr, double rh) {
        super.onUpdateHumidity(mgr, rh);
    }

    @Override
    public void onUpdateInfraredTemperature(SensorTagManager mgr, double temp) {
        super.onUpdateInfraredTemperature(mgr, temp);
    }

    @Override
    public void onUpdateKeys(SensorTagManager mgr, boolean left, boolean right) {
        super.onUpdateKeys(mgr, left, right);

        if (right) {
            mgr.calibrateMagnetometer();
        }
    }

    @Override
    public void onUpdateMagnetometer(SensorTagManager mgr, Point3D b) {
        super.onUpdateMagnetometer(mgr, b);
    }

}

So basically in the onUpdateMagnetometer method, the b contains my reading. It is of class Point3D which is shown below

public class Point3D {
public double x, y, z;

public Point3D(double x, double y, double z) {
    this.x = x;
    this.y = y;
    this.z = z;
}

public double norm() {
    return Math.sqrt(x*x + y*y + z*z);
}

@Override
public int hashCode() {
    final int prime = 31;
    int result = 1;
    long temp;
    temp = Double.doubleToLongBits(x);
    result = prime * result + (int) (temp ^ (temp >>> 32));
    temp = Double.doubleToLongBits(y);
    result = prime * result + (int) (temp ^ (temp >>> 32));
    temp = Double.doubleToLongBits(z);
    result = prime * result + (int) (temp ^ (temp >>> 32));
    return result;
}

@Override
public boolean equals(Object obj) {
    if (this == obj)
        return true;
    if (obj == null)
        return false;
    if (getClass() != obj.getClass())
        return false;
    Point3D other = (Point3D) obj;
    if (Double.doubleToLongBits(x) != Double.doubleToLongBits(other.x))
        return false;
    if (Double.doubleToLongBits(y) != Double.doubleToLongBits(other.y))
        return false;
    if (Double.doubleToLongBits(z) != Double.doubleToLongBits(other.z))
        return false;
    return true;
}

public String toString() {
    return "[" + this.x + ", " + this.y + ", " + this.z + "]";
}

}

Answer 1

One problem you have is that you're handling the 3D magnetometer as a 2D sensor. It isn't.

One way to do this is to use data from both the magnetometer AND the accelerometer:

1. You capture a reading from both sensors, run those values through SensorManager.getRotationMatrix to get an initial rotation matrix. (Note that in the documentation, the description of the matrices has the identity matrix and the rotation matrix switched.)
2. You put the result through SensorManager.getOrientation to get the device's orientation in 3D space
3. You use the first value in the orientation array as the compass direction

That final value represents the device's rotation around a vector pointed towards the center of the earth, which is exactly what you want for a compass. Using THAT value, you can then calculate the location of west, east, and south.

The code would look something like this: (ignoring the setup of the sensors)

// globals
private float[] gravityData = new float[3];
private float[] geomagneticData  = new float[3];
private boolean hasGravityData = false;
private boolean hasGeomagneticData = false;
private double rotationInDegrees;


@Override
public void onSensorChanged(SensorEvent event) {
    switch (event.sensor.getType()){
        case Sensor.TYPE_ACCELEROMETER:
            System.arraycopy(event.values, 0, gravityData, 0, 3);
            hasGravityData = true;
            break;
        case Sensor.TYPE_MAGNETIC_FIELD:
            System.arraycopy(event.values, 0, geomagneticData, 0, 3);
            hasGeomagneticData = true;
            break;
        default:
            return;
    }

    if (hasGravityData && hasGeomagneticData) {
        float identityMatrix[] = new float[9];
        float rotationMatrix[] = new float[9];
        boolean success = SensorManager.getRotationMatrix(rotationMatrix, identityMatrix,
            gravityData, geomagneticData);

        if (success) {
            float orientationMatrix[] = new float[3];
            SensorManager.getOrientation(rotationMatrix, orientationMatrix);
            float rotationInRadians = orientationMatrix[0];
            rotationInDegrees = Math.toDegrees(rotationInRadians);

            // do something with the rotation in degrees
        }
    }
}

I hope that helps!