Full Compass Bearing Code in Java Without GUI

Supporting Java SE version 7 and up
Post Reply

Was This Helpful/Useful?

Yes
2
100%
No
0
No votes
 
Total votes: 2

Duc
Fresh meat
Posts: 3
Joined: Sun Oct 21, 2012 9:10 pm
Contact:

Full Compass Bearing Code in Java Without GUI

Post by Duc »

Since I had trouble getting only the compass bearing portion to work properly for two days, after I've finally got it to work I figured I might as well share the code in case there are people out there who might need it, particularly in Java. This will save a lot of time and frustration.

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());
    }
}
Attachments
CompassConnectivity.zip
(34.08 KiB) Downloaded 669 times
User avatar
Patrick
Lead Developer
Posts: 3399
Joined: Mon Jun 20, 2005 8:46 am
Location: Canada
Contact:

Re: Full Compass Bearing Code in Java Without GUI

Post by Patrick »

Eventually, this should just be part of the library.

-Patrick
Post Reply

Who is online

Users browsing this forum: No registered users and 2 guests