PhidgetSpatial Precision - setCompassCorrectionParameters

If you notice any bugs on our site, please report them here.
ToBSn
Fresh meat
Posts: 2
Joined: Tue Jul 22, 2014 2:26 am
Contact:

PhidgetSpatial Precision - setCompassCorrectionParameters

Postby ToBSn » Tue Jul 22, 2014 2:48 am

Hi,

we are using the 1044_0 - PhidgetSpatial Precision 3/3/3.
There is an Exception when set the compass correction parameters with values from the compass-calibrator-c-2.1.8.

Same on OS X and Windows.
The prebuilt Compasscalibrator.exe on windows crashes when start calibrating.

Thanks,
Tobi

Code: Select all

using UnityEngine;
using System;
using System.IO;
using System.Collections.Generic;
using System.Threading;
using System.Text;
using Phidgets; //needed for the spatial class and the phidgets exception class
using Phidgets.Events; //needed for the phidget event handling

public class SpatialObject : MonoBehaviour
{
   public GameObject declination;
   public GameObject compass;
   public GameObject accelerator;

   public double compassBearing;
   public string bearingTxt;
   public string pitchAngleTxt;
   public string rollAngleTxt;
   
   private Quaternion rotation = new Quaternion(0,0,0,1f);
   private Quaternion compassRotation = new Quaternion(0,0,0,1f);
   private Vector3 acceleration = new Vector3(0,0,0);
   
   private double[] lastMsCount = { 0, 0, 0 };
   private bool[] lastMsCountGood = { false, false, false };
   private double[] gyroHeading = { 0, 0, 0 }; //degrees
   private double[] lastAngles = { 0, 0, 0 };
   
   private List<double[]> compassBearingFilter = new List<double[]>();
   private int compassBearingFilterSize = 10;
   
   private double ambientMagneticField = 0.57142; //Calgary
   private const double ambientGravity = 1; //in G's
   
   private Spatial spatial;
   
    void Start()
    {
        try
        {
            spatial = new Spatial();

            spatial.Attach += new AttachEventHandler(accel_Attach);
            spatial.Detach += new DetachEventHandler(accel_Detach);
            spatial.Error += new Phidgets.Events.ErrorEventHandler(accel_Error);
            spatial.SpatialData += new SpatialDataEventHandler(spatial_SpatialData);

         
         spatial.open();
            spatial.waitForAttachment();
        }
        catch (PhidgetException ex)
        {
            Debug.Log(ex.Description);
        }
    }
   
   void OnApplicationQuit() {
      if(spatial != null) {
         spatial.close();
         spatial = null;
      }
   }
   
   void FixedUpdate() {
      declination.transform.localRotation = Quaternion.Lerp(declination.transform.localRotation, rotation, Time.deltaTime);;
      compass.transform.localRotation = Quaternion.Lerp(compass.transform.localRotation, compassRotation, Time.deltaTime * 2f);
      accelerator.transform.localPosition = Vector3.Lerp(accelerator.transform.localPosition, acceleration, Time.deltaTime);
   }

    private void calculateGyroHeading(SpatialEventData[] data, int index)
    {
        double gyro = 0;
        for (int i = 0; i < data.Length; i++)
        {
            gyro = data[i].AngularRate[index];

            if (lastMsCountGood[index])
            {
                double timechange = data[i].Timestamp.TotalMilliseconds - lastMsCount[index]; // in ms
                double timeChangeSeconds = (double)timechange / 1000.0;
                gyroHeading[index] += timeChangeSeconds * gyro;
            }

            lastMsCount[index] = data[i].Timestamp.TotalMilliseconds;
            lastMsCountGood[index] = true;
        }
    }
   
    private void calculateCompassBearing()
    {
        double[] gravity = {
            spatial.accelerometerAxes[0].Acceleration,
            spatial.accelerometerAxes[1].Acceleration,
            spatial.accelerometerAxes[2].Acceleration};

        double[] magField = {
            spatial.compassAxes[0].MagneticField,
            spatial.compassAxes[1].MagneticField,
            spatial.compassAxes[2].MagneticField};

        //Roll Angle - about axis 0
        //  tan(roll angle) = gy/gz
        //  Use Atan2 so we have an output os (-180 - 180) degrees
        double rollAngle = Math.Atan2(gravity[1], gravity[2]);

        //Pitch Angle - about axis 1
        //  tan(pitch angle) = -gx / (gy * sin(roll angle) * gz * cos(roll angle))
        //  Pitch angle range is (-90 - 90) degrees
        double pitchAngle = Math.Atan(-gravity[0] / (gravity[1] * Math.Sin(rollAngle) + gravity[2] * Math.Cos(rollAngle)));

        //Yaw Angle - about axis 2
        //  tan(yaw angle) = (mz * sin(roll) – my * cos(roll)) /
        //                   (mx * cos(pitch) + my * sin(pitch) * sin(roll) + mz * sin(pitch) * cos(roll))
        //  Use Atan2 to get our range in (-180 - 180)
        //
        //  Yaw angle == 0 degrees when axis 0 is pointing at magnetic north
        double yawAngle = Math.Atan2(magField[2] * Math.Sin(rollAngle) - magField[1] * Math.Cos(rollAngle),
            magField[0] * Math.Cos(pitchAngle) + magField[1] * Math.Sin(pitchAngle) * Math.Sin(rollAngle) + magField[2] * Math.Sin(pitchAngle) * Math.Cos(rollAngle));

        double[] angles = {rollAngle, pitchAngle, yawAngle};

        //we low-pass filter the angle data so that it looks nicer on-screen
        try
        {
            //make sure the filter buffer doesn't have values passing the -180<->180 mark
            //Only for Roll and Yaw - Pitch will never have a sudden switch like that
            for(int i=0;i<3;i+=2)
            {
                if (Math.Abs(angles[i] - lastAngles[i]) > 3)
                    foreach (double[] stuff in compassBearingFilter)
                        if (angles[i] > lastAngles[i])
                            stuff[i] += 360 * Math.PI / 180.0;
                        else
                            stuff[i] -= 360 * Math.PI / 180.0;
            }

            lastAngles = (double[])angles.Clone();

            compassBearingFilter.Add((double[])angles.Clone());
            if (compassBearingFilter.Count > compassBearingFilterSize)
                compassBearingFilter.RemoveAt(0);

            yawAngle = pitchAngle = rollAngle = 0;
            foreach (double[] stuff in compassBearingFilter)
            {
                rollAngle += stuff[0];
                pitchAngle += stuff[1];
                yawAngle += stuff[2];
            }
            yawAngle /= compassBearingFilter.Count;
            pitchAngle /= compassBearingFilter.Count;
            rollAngle /= compassBearingFilter.Count;

            //bearing = (yawAngle * (180.0 / Math.PI)).ToString("F1") + "°";
            //pitchAngle = (pitchAngle * (180.0 / Math.PI)).ToString("F1") + "°";
            //rollAngle = (rollAngle * (180.0 / Math.PI)).ToString("F1") + "°";
         
         rotation.x = (float)(pitchAngle * (180.0 / Math.PI));
         rotation.y = (float)(yawAngle * (180.0 / Math.PI));
         rotation.z = (float)(rollAngle * (180.0 / Math.PI));

         //Convert radians to degrees for display
         compassBearing = yawAngle * (180.0 / Math.PI);
         
         bearingTxt = compassBearing.ToString("F1");
         pitchAngleTxt = (pitchAngle * (180.0 / Math.PI)).ToString("F1");
         rollAngleTxt = (rollAngle * (180.0 / Math.PI)).ToString("F1");
        }
        catch { }
    }
   
    private void spatial_SpatialData(object sender, SpatialDataEventArgs e)
    {
      if (spatial.accelerometerAxes.Count > 0)
        {
            //x = e.spatialData[0].Acceleration[0].ToString("F3");
            //y = e.spatialData[0].Acceleration[1].ToString("F3");
            //z = e.spatialData[0].Acceleration[2].ToString("F3");
         
         if (spatial.accelerometerAxes.Count == 3)
            {
            acceleration.x = -(float)spatial.accelerometerAxes[0].Acceleration * (float)(1 / ambientGravity);
            acceleration.y = -(float)spatial.accelerometerAxes[1].Acceleration * (float)(1 / ambientGravity);
            //acceleration.z = -(float)spatial.accelerometerAxes[2].Acceleration * (float)(1 / ambientGravity);
            }
        }

        if (spatial.gyroAxes.Count > 0)
        {
            calculateGyroHeading(e.spatialData, 0); //x axis
            calculateGyroHeading(e.spatialData, 1); //y axis
            calculateGyroHeading(e.spatialData, 2); //z axis

            //x = gyroHeading[0].ToString("F3") + "°";
            //y = gyroHeading[1].ToString("F3") + "°";
            //z = gyroHeading[2].ToString("F3") + "°";
         /*
            for (int i = 0; i < spatial.gyroAxes.Count; i++)
            {
            float x = Mathf.Cos((float)gyroHeading[i] * (Mathf.PI / 180f));
            float y = Mathf.Sin((float)gyroHeading[i] * (Mathf.PI / 180f));
            }
            */
        }

        //Even when there is a compass chip, sometimes there won't be valid data in the event.
        if (spatial.compassAxes.Count > 0 && e.spatialData[0].MagneticField.Length > 0)
        {
            //x = spatial.compassAxes[0].MagneticField.ToString("F3");
            //y = spatial.compassAxes[1].MagneticField.ToString("F3");
            //z = spatial.compassAxes[2].MagneticField.ToString("F3");
         
         if (spatial.compassAxes.Count == 3)
            {
            compassRotation.y = (float)(spatial.compassAxes[0].MagneticField) * (float)(1 / ambientMagneticField);
            }
         
            calculateCompassBearing();
        }
    }
   
    private void accel_Attach(object sender, AttachEventArgs e)
    {
        Spatial attached = (Spatial)sender;
      
      attached.DataRate = attached.DataRateMax;
      
      lastMsCountGood[0] = false;
      lastMsCountGood[1] = false;
      lastMsCountGood[2] = false;
      
      gyroHeading[0] = 0;
      gyroHeading[1] = 0;
      gyroHeading[2] = 0;

      //calibrate();
    }

   void calibrate() {
      try
      {
         //2-axis:   0.004583, 0.031603, 0.213938, 0.190835, 519.528531, 19.570946, 115.492817, -118.277968, -5.647213, -26.820496, -5.082291, -1.195689, -4.745487
         //3-axis:   0.009312, 0.040884, 0.245262, 0.141281, 265.750150, 38.296121, 18.122720, -23.034181, -7.870186, -3.092104, -0.062049, -0.699972, -0.041111
         spatial.setCompassCorrectionParameters(0.004583, 0.031603, 0.213938, 0.190835, 519.528531, 19.570946, 115.492817, -118.277968, -5.647213, -26.820496, -5.082291, -1.195689, -4.745487);
      } catch (PhidgetException ex) {
         Debug.Log(ex.Message);
      }
   }
   
   void OnGUI() {
      if(GUILayout.Button("Calibrate compass")) {
         calibrate();
      }
   }

    private void accel_Detach(object sender, DetachEventArgs e)
    {
        Debug.Log("Spatial {0} detached!" + e.Device.SerialNumber.ToString());
    }
   
    private void accel_Error(object sender, Phidgets.Events.ErrorEventArgs e)
    {
        Debug.Log(e.Description);
    }
}

User avatar
Patrick
Lead Developer
Posts: 3046
Joined: Mon Jun 20, 2005 8:46 am
Location: Canada
Contact:

Re: PhidgetSpatial Precision - setCompassCorrectionParameter

Postby Patrick » Tue Jul 22, 2014 10:05 am

Your values are really out to lunch, are you sure these came out of the compass calibration utility?

Valid ranges are:

Magnetic Field 0.1-1000
Offsets need to be 0+-5.0
Gains need to be 0-15.0
T params 0+-5.0

-Patrick

ToBSn
Fresh meat
Posts: 2
Joined: Tue Jul 22, 2014 2:26 am
Contact:

Re: PhidgetSpatial Precision - setCompassCorrectionParameter

Postby ToBSn » Wed Jul 23, 2014 12:57 am

Hi Patrick,

thanks for fast response.
I have paste the values directly from compass-calibrator-c-2.1.8.
Thats the output:

Code: Select all

Tobias:~ Tobias$ cd /Users/Tobias/Downloads/compass-calibrator-c-2.1.8
Tobias:compass-calibrator-c-2.1.8 Tobias$ ./compasscal
Choose:
   2: 2-axis calibration
   3: 3-axis calibration

::>2
Press Enter to start sampling...

Sampling... Press Enter to stop...
Captured 100 samples
Captured 200 samples
Captured 300 samples
Captured 400 samples
Captured 500 samples
Captured 600 samples
Captured 700 samples
Captured 800 samples
Captured 900 samples
Captured 1000 samples
Captured 1100 samples
Captured 1200 samples
Captured 1300 samples
Captured 1400 samples
Captured 1500 samples
Captured 1600 samples
Captured 1700 samples
Captured 1800 samples
Captured 1900 samples
Captured 2000 samples
Captured 2100 samples
Captured 2200 samples
Captured 2300 samples
Captured 2400 samples
Captured 2500 samples
Captured 2600 samples
Captured 2700 samples
Captured 2800 samples
Captured 2900 samples
Captured 3000 samples
Captured 3100 samples
Captured 3200 samples
Captured 3300 samples
Captured 3400 samples
Captured 3500 samples
Captured 3600 samples
Captured 3700 samples
Captured 3800 samples
Captured 3900 samples
Captured 4000 samples
Captured 4100 samples
Captured 4200 samples
Captured 4300 samples
Captured 4400 samples
Captured 4500 samples
Captured 4600 samples
Captured 4700 samples
Captured 4800 samples
Captured 4900 samples
Captured 5000 samples
Captured max samples - Press Enter to continue.

Z angle: 83.6
Estimates: gains(388.753, 188.888) offsets(0.091, 0.047)
.............

Compass calibration finished
   Vars: 0.030859, 0.091199, 0.046947, 0.000000, 43.618439, 21.193350, 32.405895, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000

These arguments are in the right order to be passed direcly into the CPhidgetSpatial_setCompassCorrectionParameters function.

You may wish to use a more accurate value for magnetic field strength as the number provided here is only an estimate.


Enter to exit


Tobi

User avatar
Patrick
Lead Developer
Posts: 3046
Joined: Mon Jun 20, 2005 8:46 am
Location: Canada
Contact:

Re: PhidgetSpatial Precision - setCompassCorrectionParameter

Postby Patrick » Wed Jul 23, 2014 9:46 am

These values are extremely messed up. How are you moving the 1044 while it takes the samples? Do you NEED to use the 2-axis calibration? a 3-axis gives much better results. Is there any chance you could use the graphical c# compass calibrator? This gives you visual feedback as you move the compass while the samples are being taken, so you know when you have a nice even distribution of samples. This is prebuilt and installed into program files/phidgets/examples by the windows installer. Also, make sure you are using the latest library and compass calibrator code, there were some fixes a while back.

-Patrick


Return to “Bug Report”

Who is online

Users browsing this forum: No registered users and 0 guests