Additionally you must calibrate your compass based on your region in order to get the correct readings/outputs. Follow link below.
http://www.phidgets.com/docs/Compass_Primer#Calibration
I've attached the full working project that can be run as is in NetBeans. My hope is someone out there will find this useful and save them time.
Preview here:
Code: Select all
package compassConnectivity;
import com.phidgets.event.*;
import Math.Matrix3x3;
import Math.Vector3;
import com.phidgets.SpatialEventData;
import com.phidgets.SpatialPhidget;
import com.phidgets.PhidgetException;
import com.phidgets.event.SpatialDataListener;
import com.phidgets.event.SpatialDataEvent;
import java.io.IOException;
import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.ArrayList;
public class Compass
{
private String magX;
private String magY;
private String magZ;
private Double lastTime = 0.0;
private String pitchAngleStr;
private String rollAngleStr;
private String bearingStr;
private ArrayList<Double[]> compassBearingFilter;
private double compassBearing = 0.0;
private final char DEGREESYMBOL = '\u00b0';
private SpatialPhidget spatial;
public Compass() throws PhidgetException, IOException
{
compassBearingFilter = new ArrayList<Double[]>();
spatial = new SpatialPhidget();
spatial.addAttachListener(new AttachListener()
{
public void attached(AttachEvent ae)
{
System.out.println("attachment of " + ae);
try
{
// You will need to calibrate your compass for correct readings.
// Set the compass correction parameters based on your own calibration.
//((SpatialPhidget)ae.getSource()).setCompassCorrectionParameters(0.21834, -0.07248, 0.14129, 0.22252, 2.47457, 2.53446, 8.73118, 0.00116, -0.19671, 0.00054, 0.04786, -0.56484, 0.18815);
((SpatialPhidget)ae.getSource()).setDataRate(4);
}
catch (PhidgetException pe)
{
System.out.println("Problem setting data rate!");
}
}
});
spatial.addDetachListener(new DetachListener()
{
public void detached(DetachEvent ae)
{
System.out.println("detachment of " + ae);
}
});
spatial.addErrorListener(new ErrorListener()
{
public void error(ErrorEvent ee)
{
System.out.println("error event for " + ee);
}
});
spatial.addSpatialDataListener(new SpatialDataListener()
{
public void data(SpatialDataEvent sde)
{
try
{
//Even when there is a compass chip, sometimes there won't be valid data in the event.
if ((spatial.getCompassAxisCount() > 0) && (sde.getData()[0].getMagneticField().length > 0))
{
magX = Double.toString(roundDouble((sde.getData()[0].getMagneticField()[0]), 3));
magY = Double.toString(roundDouble((sde.getData()[0].getMagneticField()[1]), 3));
magZ = Double.toString(roundDouble((sde.getData()[0].getMagneticField()[2]), 3));
System.out.println("magX = " + magX);
System.out.println("magY = " + magY);
System.out.println("magZ = " + magZ);
try
{
calculateCompassBearing(sde.getData());
}
catch (Exception ex)
{
}
}
}
catch (Exception ex)
{
}
}
});
spatial.openAny();
System.out.println("waiting for Spatial attachment...");
spatial.waitForAttachment();
System.out.println("Outputting events. Input to stop.");
System.in.read();
System.out.print("closing...");
spatial.close();
spatial = null;
System.out.println(" ok");
}
public double getCompassBearing()
{
return compassBearing;
}
//This finds a magnetic north bearing, correcting for board tilt and roll as measured by the accelerometer
//This doesn't account for dynamic acceleration - ie accelerations other then gravity will throw off the calculation
double lastBearing = 0;
public void calculateCompassBearing(SpatialEventData[] spatialData)
{
double Xh = 0;
double Yh = 0;
//find the tilt of the board with respect to gravity
Vector3 gravity = Vector3.Normalize(new Vector3(
spatialData[0].getAcceleration()[0],
spatialData[0].getAcceleration()[2],
spatialData[0].getAcceleration()[1]));
double pitchAngle = Math.asin(gravity.X);
double rollAngle = Math.asin(gravity.Z);
//The board is up-side down
if (gravity.Y < 0) {
pitchAngle = -pitchAngle;
rollAngle = -rollAngle;
}
//Construct a rotation matrix for rotating vectors measured in the body frame, into the earth frame
//this is done by using the angles between the board and the gravity vector.
Matrix3x3 xRotMatrix = new Matrix3x3();
xRotMatrix.matrix[0][0] = Math.cos(pitchAngle);
xRotMatrix.matrix[1][0] = -Math.sin(pitchAngle);
xRotMatrix.matrix[2][0] = 0;
xRotMatrix.matrix[0][1] = Math.sin(pitchAngle);
xRotMatrix.matrix[1][1] = Math.cos(pitchAngle);
xRotMatrix.matrix[2][1] = 0;
xRotMatrix.matrix[0][2] = 0;
xRotMatrix.matrix[1][2] = 0;
xRotMatrix.matrix[2][2] = 1;
Matrix3x3 zRotMatrix = new Matrix3x3();
zRotMatrix.matrix[0][0] = 1;
zRotMatrix.matrix[1][0] = 0;
zRotMatrix.matrix[2][0] = 0;
zRotMatrix.matrix[0][1] = 0;
zRotMatrix.matrix[1][1] = Math.cos(rollAngle);
zRotMatrix.matrix[2][1] = -Math.sin(rollAngle);
zRotMatrix.matrix[0][2] = 0;
zRotMatrix.matrix[1][2] = Math.sin(rollAngle);
zRotMatrix.matrix[2][2] = Math.cos(rollAngle);
Matrix3x3 rotMatrix = Matrix3x3.Multiply(xRotMatrix, zRotMatrix);
Vector3 data = new Vector3(
spatialData[0].getMagneticField()[0],
spatialData[0].getMagneticField()[2],
-spatialData[0].getMagneticField()[1]);
Vector3 correctedData = Matrix3x3.Multiply(data, rotMatrix);
//These represent the x and y components of the magnetic field vector in the earth frame
Xh = -correctedData.Z;
Yh = -correctedData.X;
//we use the computed X-Y to find a magnetic North bearing in the earth frame
try {
double bearing = 0.0;
double _360inRads = (360.0 * Math.PI / 180.0);
if (Xh < 0.0) {
bearing = Math.PI - Math.atan(Yh / Xh);
} else if ((Xh > 0.0) && (Yh < 0.0)) {
bearing = -Math.atan(Yh / Xh);
} else if ((Xh > 0.0) && (Yh > 0.0)) {
bearing = Math.PI * 2.0 - (Math.atan(Yh / Xh));
} else if ((Xh == 0.0) && (Yh < 0.0)) {
bearing = Math.PI / 2.0;
} else if ((Xh == 0.0) && (Yh > 0.0)) {
bearing = Math.PI * 1.5;
}
//The board is up-side down
if (gravity.Y < 0) {
bearing = Math.abs(bearing - _360inRads);
}
//passing the 0 <-> 360 point, need to make sure the filter never contains both values near 0 and values near 360 at the same time.
if (Math.abs(bearing - lastBearing) > 2) //2 radians == ~115 degrees
{
if (bearing > lastBearing) {
for (Double[] stuff : compassBearingFilter) {
stuff[0] += _360inRads;
}
} else {
for (Double[] stuff : compassBearingFilter) {
stuff[0] -= _360inRads;
}
}
}
Double[] temp = {bearing, pitchAngle, rollAngle};
int compassBearingFilterSize = 10;
compassBearingFilter.add(temp);
if (compassBearingFilter.size() > compassBearingFilterSize)
{
compassBearingFilter.remove(0);
}
bearing = 0;
pitchAngle = 0;
rollAngle = 0;
for (Double[] stuff : compassBearingFilter)
{
bearing += stuff[0];
pitchAngle += stuff[1];
rollAngle += stuff[2];
}
bearing /= compassBearingFilter.size();
pitchAngle /= compassBearingFilter.size();
rollAngle /= compassBearingFilter.size();
compassBearing = bearing * (180.0 / Math.PI);
System.out.println("compassBearing = " + compassBearing);
lastBearing = bearing;
bearingStr = Double.toString(roundDouble(bearing * (180.0 / Math.PI), 1));
pitchAngleStr = Double.toString(roundDouble(pitchAngle * (180.0 / Math.PI), 1));
rollAngleStr = Double.toString(roundDouble(rollAngle * (180.0 / Math.PI), 1));
}
catch (Exception ex)
{
}
}
private static double roundDouble(Double value, int decimalPlaces)
{
BigDecimal bd = new BigDecimal(value).setScale(decimalPlaces, RoundingMode.HALF_EVEN);
return (bd.doubleValue());
}
}
