Example #1
0
 //spatial data handler - all spatial data tied together.
 static void spatial_SpatialData(object sender, SpatialDataEventArgs e)
 {
     Console.WriteLine("SpatialData event time:" + e.spatialData[0].Timestamp.TotalSeconds.ToString());
     if (e.spatialData[0].Acceleration.Length > 0)
     {
         Console.WriteLine(" Acceleration: " + e.spatialData[0].Acceleration[0] + ", " + e.spatialData[0].Acceleration[1] + ", " + e.spatialData[0].Acceleration[2]);
     }
     if (e.spatialData[0].AngularRate.Length > 0)
     {
         Console.WriteLine(" Angular Rate: " + e.spatialData[0].AngularRate[0] + ", " + e.spatialData[0].AngularRate[1] + ", " + e.spatialData[0].AngularRate[2]);
     }
     if (e.spatialData[0].MagneticField.Length > 0)
     {
         Console.WriteLine(" Magnetic Field: " + e.spatialData[0].MagneticField[0] + ", " + e.spatialData[0].MagneticField[1] + ", " + e.spatialData[0].MagneticField[2]);
     }
 }
Example #2
0
        void spatial_SpatialData(object sender, SpatialDataEventArgs e)
        {
            try
            {
                if (spatial.accelerometerAxes.Count > 0)
                {
                    xTextBox.Text = e.spatialData[0].Acceleration[0].ToString("F3");
                    yTextBox.Text = e.spatialData[0].Acceleration[1].ToString("F3");
                    zTextBox.Text = e.spatialData[0].Acceleration[2].ToString("F3");
                    drawAccelGraph();
                }

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

                    headingXText.Text = gyroHeading[0].ToString("F3") + "°";
                    headingYText.Text = gyroHeading[1].ToString("F3") + "°";
                    headingZText.Text = gyroHeading[2].ToString("F3") + "°";

                    drawGyroGraph();
                }

                //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)
                {
                    compassXTxt.Text = spatial.compassAxes[0].MagneticField.ToString("F3");
                    compassYTxt.Text = spatial.compassAxes[1].MagneticField.ToString("F3");
                    compassZTxt.Text = spatial.compassAxes[2].MagneticField.ToString("F3");

                    try
                    {
                        drawMagFieldGraph();
                        calculateCompassBearing();
                        drawCompassBearingGraph();
                    }
                    catch
                    {
                    }
                }
            }
            catch { }
        }
Example #3
0
    // Use for calculations using sensors
    public void spatial_SpatialData(object sender, SpatialDataEventArgs e)
    {
        // Negative logic
        if (spatial.accelerometerAxes.Count != 3 || spatial.gyroAxes.Count != 3 || spatial.compassAxes.Count != 3)
        {
            return;
        }

        if (timestampLastUpdate != DateTime.MinValue)
        {
            // Conversion into correct units
            gx = (float)spatial.gyroAxes[0].AngularRate * 0.0174532925f;
            gy = (float)spatial.gyroAxes[1].AngularRate * 0.0174532925f;
            gz = (float)spatial.gyroAxes[2].AngularRate * 0.0174532925f;

            float seconds = (float)(DateTime.Now - timestampLastUpdate).TotalSeconds;

            madgwick.SamplePeriod = (float)(seconds);
            if (Alg == AlgorithmUsed.AHRSAlg)
            {
                AHRSUpdate(gx, gy, gz, ax, ay, az, mx, my, mz);
            }
            else if (Alg == AlgorithmUsed.CompassGravAlg)
            {
                CompassGrav(ax, ay, az);
            }
            //CompassGrav(ax, ay, az);
        }

        timestampLastUpdate = DateTime.Now;

        ax = (float)e.spatialData[0].Acceleration[0];
        ay = (float)e.spatialData[0].Acceleration[1];
        az = (float)e.spatialData[0].Acceleration[2];

        mx = (float)spatial.compassAxes[0].MagneticField;
        my = (float)spatial.compassAxes[1].MagneticField;
        mz = (float)spatial.compassAxes[2].MagneticField;

        CalculateCompassBearing();
    }
        // Use for calculations using sensors
        private void spatial_SpatialData(object sender, SpatialDataEventArgs e)
        {
            if (phidgetsAttached)
            {
                if (spatial.accelerometerAxes.Count != 3 || spatial.gyroAxes.Count != 3 || spatial.compassAxes.Count != 3)
                {
                    return;
                }

                if (timestampLastUpdate != DateTime.MinValue)
                {
                    //ax = (float)e.spatialData[0].Acceleration[0];
                    //ay = (float)e.spatialData[0].Acceleration[1];
                    //az = (float)e.spatialData[0].Acceleration[2];
                    ax = e.spatialData[0].Acceleration[0];
                    ay = e.spatialData[0].Acceleration[1];
                    az = e.spatialData[0].Acceleration[2];
                    Vector3 acc = new Vector3((float)ax, (float)ay, (float)az);
                    //Debug.Log("BEFORE FILTER: " + ax + " " + ay + " " + az );

                    // Conversion into correct units
                    //gx = (float)spatial.gyroAxes[0].AngularRate * 0.0174532925f;
                    //gy = (float)spatial.gyroAxes[1].AngularRate * 0.0174532925f;
                    //gz = (float)spatial.gyroAxes[2].AngularRate * 0.0174532925f;

                    //mx = (float)spatial.compassAxes[0].MagneticField;
                    //my = (float)spatial.compassAxes[1].MagneticField;
                    //mz = (float)spatial.compassAxes[2].MagneticField;
                    mx = spatial.compassAxes[0].MagneticField;
                    my = spatial.compassAxes[1].MagneticField;
                    mz = spatial.compassAxes[2].MagneticField;

                    gx = spatial.gyroAxes[0].AngularRate * 0.0174532925f;
                    gy = spatial.gyroAxes[1].AngularRate * 0.0174532925f;
                    gz = spatial.gyroAxes[2].AngularRate * 0.0174532925f;
                    Vector3 gyr = new Vector3((float)gx, (float)gy, (float)gz); //new is very very important

                    float seconds = (float)(DateTime.Now - timestampLastUpdate).TotalSeconds;
                    //madgwick.SamplePeriod = (float)(seconds);
                    madgwick.SamplePeriod = (float)mSamplePeriod;

                    accelBeforeFilter.WriteLine("" + ax + "," + ay + "," + az);

                    // Filter Acc data
                    xValuesAcc.Add(acc);
                    if (xValuesAcc.Count > (order + 1))
                    {
                        xValuesAcc.RemoveAt(0);
                    }

                    if (xValuesAcc.Count == (order + 1))
                    {
                        // filter
                        Vector3 yValue = mButterLP.applyButterWorth(xValuesAcc, yValuesAcc);
                        yValuesAcc.Add(yValue);
                    }
                    else
                    {
                        yValuesAcc.Add(acc);
                    }

                    if (yValuesAcc.Count > order)
                    {
                        yValuesAcc.RemoveAt(0);
                    }


                    //Filter Gyro Data
                    xValuesGyr.Add(gyr);
                    if (xValuesGyr.Count > (order + 1))
                    {
                        xValuesGyr.RemoveAt(0);
                    }

                    if (xValuesGyr.Count == (order + 1))
                    {
                        // filter
                        Vector3 yValue = mButterLP.applyButterWorth(xValuesGyr, yValuesGyr);
                        yValuesGyr.Add(yValue);
                    }
                    else
                    {
                        yValuesGyr.Add(gyr);
                    }

                    if (yValuesGyr.Count > order)
                    {
                        yValuesGyr.RemoveAt(0);
                    }

                    int sz        = order;
                    int lastIndex = yValuesGyr.Count - 1;

                    //Debug.Log(yValuesGyr[lastIndex].x + ", " + yValuesGyr[lastIndex].y + ", " +yValuesGyr[lastIndex].z + ", ");

                    gx = yValuesGyr[lastIndex].x;
                    gy = yValuesGyr[lastIndex].y;
                    gz = yValuesGyr[lastIndex].z;


                    lastIndex = yValuesAcc.Count - 1;
                    ax        = yValuesAcc[lastIndex].x;
                    ay        = yValuesAcc[lastIndex].y;
                    az        = yValuesAcc[lastIndex].z;

                    accelAfterFilter.WriteLine("" + ax + "," + ay + "," + az);

                    AHRSUpdate(gx, gy, gz, ax, ay, az);

                    Quaternion mappedQuat;
                    mappedQuat = new Quaternion(-madgwick.Quaternion[1], madgwick.Quaternion[2], madgwick.Quaternion[3], madgwick.Quaternion[0]);
                    //mMatrix.SetTRS(Vector3.zero, mappedQuat, new Vector3(1, 1, 1));

                    mMatrix = QuaternionToMatrix(mappedQuat);
                    rMatrix = mMatrix;
                    Matrix4x4 aMatrix = mMatrix.inverse;

                    //print((mMatrix * aMatrix).ToString());


                    //print(madgwick.Quaternion[0] + " " + madgwick.Quaternion[1] + " " + madgwick.Quaternion[2] + " " + madgwick.Quaternion[3]);
                    //print(mMatrix.ToString());
                    ////print(mMatrix[0, 0] + ", " + mMatrix[0, 1] + ", " + mMatrix[0, 2] + ", " + mMatrix[0, 3] + ", ");
                    ////print(mMatrix[1, 0] + ", " + mMatrix[1, 1] + ", " + mMatrix[1, 2] + ", " + mMatrix[1, 3] + ", ");
                    ////print(mMatrix[1, 0] + ", " + mMatrix[2, 1] + ", " + mMatrix[2, 2] + ", " + mMatrix[2, 3] + ", ");
                    ////print(mMatrix[1, 0] + ", " + mMatrix[3, 1] + ", " + mMatrix[3, 2] + ", " + mMatrix[3, 3] + ", ");

                    /* Not getting the correct acceleration in x. Multiplying acc vector with inverse of rmatrix does not give correct acc in x */
                    //accelerometer in Earth frame
                    mTcAcc = new Vector3((float)yValuesAcc[lastIndex].x, (float)yValuesAcc[lastIndex].y, (float)yValuesAcc[lastIndex].z);
                    //print("before: " + mTcAcc.x + ", " + mTcAcc.y + ", " + mTcAcc.z + ", ");

                    //mTcAcc = aMatrix.MultiplyPoint(mTcAcc);
                    mTcAcc = VecMulMatrix(mTcAcc, aMatrix);
                    //print("mTcAcc: " + mTcAcc.x + ", " + mTcAcc.y + ", " +  mTcAcc.z + ", ");

                    ////linear acceleration in earth frame
                    mTcAcc -= new Vector3(0f, 0f, 1f);
                    //print("mTcAcc: " + mTcAcc.x + ", " + mTcAcc.y + ", " + mTcAcc.z + ", ");


                    mLinAcc = (mTcAcc * (9.81f));
                    //print("after: " + mLinAcc.x + ", " + mLinAcc.y + ", " + mLinAcc.z + ", ");

                    ///* Linear velocity */
                    mLinVel     = mLinVelPrev + mLinAcc * (float)mSamplePeriod;//(float)(seconds); //
                    mLinVelPrev = mLinVel;

                    ////print("before : velx  " + mLinVel.x + "," + "vely " + mLinVel.y + "," + "velz  " + mLinVel.z + ",");

                    velocityBeforeFilter.WriteLine(mLinVel.x + "," + mLinVel.y + "," + mLinVel.z);

                    ////high pass filter linear velocity to remove drift
                    ////push the lasted to the end and delete the first
                    Vector3 temp = new Vector3();
                    temp = mLinVel;

                    xValuesVel.Add(temp);

                    if (xValuesVel.Count > (order + 1))
                    {
                        xValuesVel.RemoveAt(0);
                    }

                    if (xValuesVel.Count == (order + 1))
                    {
                        //filter
                        Vector3 yValue = mButterHP.applyButterWorth(xValuesVel, yValuesVel);
                        yValuesVel.Add(yValue);
                    }
                    else
                    {
                        yValuesVel.Add(mLinVel);
                    }

                    if (yValuesVel.Count > order)
                    {
                        yValuesVel.RemoveAt(0);
                    }

                    lastIndex = yValuesVel.Count - 1;

                    velocityAfterFilter.WriteLine("" + yValuesVel[lastIndex].x + "," + yValuesVel[lastIndex].y + "," + yValuesVel[lastIndex].z + ",");

                    ///* Linear position */
                    mLinPos     = (mLinPosPrev + (yValuesVel[yValuesVel.Count - 1] * (float)(mSamplePeriod)));//(float)(seconds)));//
                    mLinPosPrev = (mLinPos);

                    //high pass filter linear position to remove drift
                    Vector3 temppos = new Vector3();
                    temppos = (mLinPos);

                    xValuesPos.Add(temppos);
                    if (xValuesPos.Count > (order + 1))
                    {
                        xValuesPos.RemoveAt(0);
                    }

                    if (xValuesPos.Count == (order + 1))
                    {
                        //filter
                        Vector3 yValue = mButterHP.applyButterWorth(xValuesPos, yValuesPos);
                        yValuesPos.Add(yValue);
                    }
                    else
                    {
                        yValuesPos.Add(mLinPos);
                    }

                    if (yValuesPos.Count > order)
                    {
                        yValuesPos.RemoveAt(0);
                    }

                    pX = yValuesPos[yValuesPos.Count - 1].x;
                    pY = yValuesPos[yValuesPos.Count - 1].y;
                    pZ = yValuesPos[yValuesPos.Count - 1].z;
                }

                /*
                 *
                 * Positional calculation still bugged.
                 *
                 */

                timestampLastUpdate = DateTime.Now;


                CalculateCompassBearing();
            }
        }
Example #5
0
 protected virtual void SpatialData(object sender, SpatialDataEventArgs e)
 {
     if (spatial.accelerometerAxes.Count > 0)
         {
         //Acc[0] = x, Acc[1] = y, Acc[2]= z
             acc.x = e.spatialData[0].Acceleration[2];
             acc.y = e.spatialData[0].Acceleration[1];
             acc.z = e.spatialData[0].Acceleration[0];
         }
     //			Console.WriteLine(acc);
 }
 // Record Orientation
 //spatial data handler - all spatial data tied together.
 void spatial_SpatialData(object sender, SpatialDataEventArgs e)
 {
     CurrTime = DateTime.Now;
     twPhidgetTimestamp.WriteLine(CurrTime.ToString("yyyy-MM-dd HH:mm:ss.fffffff"));
     twPhidgetTimestamp.WriteLine("SpatialData event time:" + e.spatialData[0].Timestamp.TotalSeconds.ToString());
     if (e.spatialData[0].Acceleration.Length > 0)
         twPhidgetTimestamp.WriteLine(" Acceleration: " + e.spatialData[0].Acceleration[0] + ", " + e.spatialData[0].Acceleration[1] + ", " + e.spatialData[0].Acceleration[2]);
     if (e.spatialData[0].AngularRate.Length > 0)
         twPhidgetTimestamp.WriteLine(" Angular Rate: " + e.spatialData[0].AngularRate[0] + ", " + e.spatialData[0].AngularRate[1] + ", " + e.spatialData[0].AngularRate[2]);
     if (e.spatialData[0].MagneticField.Length > 0)
         twPhidgetTimestamp.WriteLine(" Magnetic Field: " + e.spatialData[0].MagneticField[0] + ", " + e.spatialData[0].MagneticField[1] + ", " + e.spatialData[0].MagneticField[2]);
 }
Example #7
0
        void phid_SpatialData(object sender, SpatialDataEventArgs e)
        {
            try
            {
                Pipeline.fillpen = Color.White;
                for (int i = 0; i < e.spatialData.Length; i++)
                {
                    if (zeroing)
                    {
                        if(e.spatialData[i].MagneticField.Length>0)
                        {
                            magTemp.X += e.spatialData[i].MagneticField[0];
                            magTemp.Y += e.spatialData[i].MagneticField[2];
                            magTemp.Z -= e.spatialData[i].MagneticField[1];
                            magSamplesTaken++;
                        }

                        gravityTemp.X += e.spatialData[i].Acceleration[0];
                        gravityTemp.Y += e.spatialData[i].Acceleration[2];
                        gravityTemp.Z -= e.spatialData[i].Acceleration[1];
                        gravitySamplesTaken++;

                        if (gravitySamplesTaken >= gravitySamples)
                        {
                            gravityTemp.X /= gravitySamplesTaken;
                            gravityTemp.Y /= gravitySamplesTaken;
                            gravityTemp.Z /= gravitySamplesTaken;
                            gravityRef = gravityTemp;

                            gravityTemp = new Vector3(0, 0, 0);
                            gravitySamplesTaken = 0;

                            magTemp.X /= magSamplesTaken;
                            magTemp.Y /= magSamplesTaken;
                            magTemp.Z /= magSamplesTaken;
                            magRef = new Vector3(magTemp.X, magTemp.Y, magTemp.Z);

                            magTemp = new Vector3(0, 0, 0);
                            magSamplesTaken = 0;

                            zeroing = false;
                            finishZeroing();
                        }
                    }
                    else
                    {
                        if (lastMsCountGood)
                        {
                            double timechange = e.spatialData[i].Timestamp.TotalMilliseconds - lastMsCount; // in ms
                            double timeChangeSeconds = (double)timechange / 1000.0;

                            calculateAttitude(e.spatialData[i], timeChangeSeconds);
                            calculatePosition(e.spatialData[i], timeChangeSeconds);
                        }
                    }

                    lastMsCount = e.spatialData[i].Timestamp.TotalMilliseconds;
                    lastMsCountGood = true;

                    //sw.WriteLine(e.spatialData[i].Timestamp.TotalSeconds.ToString() + "," + e.spatialData[i].Acceleration[0] + "," + -e.spatialData[i].Acceleration[2] + "," + e.spatialData[i].Acceleration[1]);
                }
            }
            catch { }
        }
Example #8
0
        // Use for calculations using sensors
        private void spatial_SpatialData(object sender, SpatialDataEventArgs e)
        {
            if (phidgetsAttached)
            {
                if (spatial.accelerometerAxes.Count != 3 || spatial.gyroAxes.Count != 3 || spatial.compassAxes.Count != 3)
                {
                    return;
                }

                //ax = (float)e.spatialData[0].Acceleration[0];
                //ay = (float)e.spatialData[0].Acceleration[1];
                //az = (float)e.spatialData[0].Acceleration[2];
                ax = e.spatialData[0].Acceleration[0];
                ay = e.spatialData[0].Acceleration[1];
                az = e.spatialData[0].Acceleration[2];
                Vector3 acc = new Vector3((float)ax, (float)ay, (float)az);

                // Conversion into correct units
                //gx = (float)spatial.gyroAxes[0].AngularRate * 0.0174532925f;
                //gy = (float)spatial.gyroAxes[1].AngularRate * 0.0174532925f;
                //gz = (float)spatial.gyroAxes[2].AngularRate * 0.0174532925f;


                //mx = (float)spatial.compassAxes[0].MagneticField;
                //my = (float)spatial.compassAxes[1].MagneticField;
                //mz = (float)spatial.compassAxes[2].MagneticField;
                mx = spatial.compassAxes[0].MagneticField;
                my = spatial.compassAxes[1].MagneticField;
                mz = spatial.compassAxes[2].MagneticField;

                if (timestampLastUpdate != DateTime.MinValue)
                {
                    gx = spatial.gyroAxes[0].AngularRate * 0.0174532925f;
                    gy = spatial.gyroAxes[1].AngularRate * 0.0174532925f;
                    gz = spatial.gyroAxes[2].AngularRate * 0.0174532925f;
                    Vector3 gyr = new Vector3((float)gx, (float)gy, (float)gz); //new is very very important
                    //Debug.Log(ax + " " + ay + " " + az + " " + mx + " " + my + " " + mz + " " + gx + " " + gy + " " + gz);

                    float seconds = (float)(DateTime.Now - timestampLastUpdate).TotalSeconds;
                    madgwick.SamplePeriod = (float)(seconds);
                    AHRSUpdate(gx, gy, gz, ax, ay, az);//, mx, my, mz);

                    /*
                     *
                     * Positional calculation still bugged.
                     *
                     */

                    ////filter the gyro data
                    //xValuesAcc.Add(acc);
                    //if (xValuesAcc.Count > (order + 1))
                    //{
                    //  //Debug.Log("remove");
                    //  xValuesAcc.RemoveAt(0);
                    //}

                    //if (xValuesAcc.Count == (order + 1))
                    //{
                    //  //filter
                    //  //Debug.Log("filter");
                    //  Vector3 yValue = mButterLP.applyButterWorth(xValuesAcc, yValuesAcc);
                    //  yValuesAcc.Add(yValue);
                    //}
                    //else
                    //{
                    //  // Debug.Log("add");
                    //  yValuesAcc.Add(acc);
                    //}


                    //if (yValuesAcc.Count > order)
                    //{
                    //  yValuesAcc.RemoveAt(0);
                    //}

                    ////filter the gyr data
                    //xValuesGyr.Add(gyr);
                    //if (xValuesGyr.Count > (order + 1))
                    //{
                    //  xValuesGyr.RemoveAt(0);
                    //}

                    //if (xValuesGyr.Count == (order + 1))
                    //{
                    //  //filter
                    //  Vector3 yValue = mButterLP.applyButterWorth(xValuesGyr, yValuesGyr);
                    //  yValuesGyr.Add(yValue);
                    //}
                    //else
                    //{
                    //  yValuesGyr.Add(gyr);
                    //}

                    //if (yValuesGyr.Count > order)
                    //{
                    //  yValuesGyr.RemoveAt(0);
                    //}

                    //int sz = order;

                    //int lastIndex = yValuesGyr.Count - 1;
                    //gx = yValuesGyr[lastIndex].x;
                    //gy = yValuesGyr[lastIndex].y;
                    //gz = yValuesGyr[lastIndex].z;

                    //lastIndex = yValuesAcc.Count - 1;
                    //ax = yValuesAcc[lastIndex].x;
                    //ay = yValuesAcc[lastIndex].y;
                    //az = yValuesAcc[lastIndex].z;


                    //float seconds = (float)(DateTime.Now - timestampLastUpdate).TotalSeconds;

                    //madgwick.SamplePeriod = (float)seconds;

                    //AHRSUpdate(gx, gy, gz, ax, ay, az);
                    ////AHRSUpdate(gx, gy, gz, ax, ay, az, mx, my, mz);   //PROBLEM: Coordinates mis-mapped with acc/gyro coordinates.
                    ////// AHRS quaternion is in [w,pitch,roll,yaw]?
                    ////print(madgwick.Quaternion[2] + ", " + madgwick.Quaternion[1] + ", " + madgwick.Quaternion[3] + ", " + madgwick.Quaternion[0] + ", ");

                    ////// Quaternion returned in [z,y,x,w]?
                    ////Quaternion mappedQuat = new Quaternion(-madgwick.Quaternion[2], madgwick.Quaternion[1], madgwick.Quaternion[0], madgwick.Quaternion[3]);
                    ////// Mapping to [x,y,z,w]
                    ////Quaternion mappedQuat = new Quaternion(madgwick.Quaternion[1], madgwick.Quaternion[2], -madgwick.Quaternion[3], madgwick.Quaternion[0]);
                    ////Quaternion mappedQuat = new Quaternion(madgwick.Quaternion[2], madgwick.Quaternion[1], madgwick.Quaternion[3], madgwick.Quaternion[0]);
                    ////Quaternion mappedQuat = new Quaternion(madgwick.Quaternion[3], madgwick.Quaternion[2], madgwick.Quaternion[1], madgwick.Quaternion[0]);
                    //Quaternion mappedQuat = new Quaternion(madgwick.Quaternion[2], madgwick.Quaternion[3], madgwick.Quaternion[1], madgwick.Quaternion[0]);

                    ////print(mappedQuat.x + ", " + mappedQuat.y + ", " + mappedQuat.z + ", " + mappedQuat.w);

                    ////Vector3 euler = Quaternion.ToEulerAngles(mappedQuat);
                    ////print("euler:" + euler);

                    ////Convert Quaternion to Matrix
                    //mMatrix = QuaternionToMatrix(mappedQuat);

                    //mMatrix.SetTRS(Vector3.zero, mappedQuat, new Vector3(1, 1, 1));
                    ////print(mMatrix[0, 0] + ", " + mMatrix[0, 1] + ", " + mMatrix[0, 2] + ", " + mMatrix[0, 3] + ", ");
                    ////print(mMatrix[1, 0] + ", " + mMatrix[1, 1] + ", " + mMatrix[1, 2] + ", " + mMatrix[1, 3] + ", ");
                    ////print(mMatrix[2, 0] + ", " + mMatrix[2, 1] + ", " + mMatrix[2, 2] + ", " + mMatrix[2, 3] + ", ");
                    ////print(mMatrix[3, 0] + ", " + mMatrix[3, 1] + ", " + mMatrix[3, 2] + ", " + mMatrix[3, 3] + ", ");


                    ////accelerometer in Earth frame
                    //mTcAcc = yValuesAcc[lastIndex];
                    ////print("before: " + mTcAcc.x + ", " + mTcAcc.y + ", " + mTcAcc.z + ", ");

                    //mTcAcc = VecMulMatrix(mTcAcc, mMatrix.inverse);
                    ////print("after: " + mTcAcc.x + ", " + mTcAcc.y + ", " +  mTcAcc.z + ", ");


                    ////linear acceleration in earth frame
                    //mTcAcc = mTcAcc - new Vector3(0, 0, 1);
                    //mLinAcc = mTcAcc * 9.81f;
                    ////print("after: " + mLinAcc.x + ", " + mLinAcc.y + ", " + mLinAcc.z + ", ");


                    ////mLinVel = (mLinVelPrev + mLinAcc * (float)mSamplePeriod); // Using mSamplePeriod
                    //mLinVel = (mLinVelPrev + mLinAcc * madgwick.SamplePeriod);
                    //mLinVelPrev = new Vector3(mLinVel.x, mLinVel.y, mLinVel.z);
                    ////print("before : velx  " + mLinVel.x + ","  + "vely " + mLinVel.y + "," + "velz  " + mLinVel.z + ",");


                    ////high pass filter linear velocity to remove drift
                    ////push the lasted to the end and delete the first
                    //Vector3 temp = new Vector3(mLinVel.x, mLinVel.y, mLinVel.z);

                    //xValuesVel.Add(temp);

                    //if (xValuesVel.Count > (order + 1))
                    //{
                    //  xValuesVel.RemoveAt(0);
                    //}
                    //if (xValuesVel.Count == (order + 1))
                    //{
                    //  ////filter
                    //  Vector3 yValue = mButterHP.applyButterWorth(xValuesVel, yValuesVel);
                    //  yValuesVel.Add(yValue);
                    //}
                    //else
                    //{
                    //  yValuesVel.Add(mLinVel);
                    //}

                    //if (yValuesVel.Count > order)
                    //{
                    //  yValuesVel.RemoveAt(0);
                    //}
                    //////here, the size of xs is order+1, the size of ys is order

                    ////printout


                    ////// Linear position
                    ////mLinPos = (mLinPosPrev + yValuesVel[yValuesVel.Count - 1] * (float)mSamplePeriod);
                    //mLinPos = (mLinPosPrev + yValuesVel[yValuesVel.Count - 1] * madgwick.SamplePeriod);
                    //mLinPosPrev = mLinPos;

                    //////high pass filter linear position to remove drift
                    //Vector3 temppos = new Vector3(mLinPos.x, mLinPos.y, mLinPos.z);

                    //xValuesPos.Add(temppos);
                    //if (xValuesPos.Count > (order + 1))
                    //{
                    //  xValuesPos.RemoveAt(0);
                    //}

                    //if (xValuesPos.Count == (order + 1))
                    //{
                    //  ////filter
                    //  Vector3 yValue = mButterHP.applyButterWorth(xValuesPos, yValuesPos);
                    //  yValuesPos.Add(yValue);
                    //}
                    //else
                    //{
                    //  yValuesPos.Add(mLinPos);
                    //}

                    //if (yValuesPos.Count > order)
                    //{
                    //  yValuesPos.RemoveAt(0);
                    //}

                    //////result of pos
                    //pX = yValuesPos[yValuesPos.Count - 1].x;
                    //pY = yValuesPos[yValuesPos.Count - 1].y;
                    //pZ = yValuesPos[yValuesPos.Count - 1].z;

                    //print("Position change:" + pX + " " + pY + " " + pZ);
                    //pos = new Vector3((float)pX, (float)pY, (float)pZ);
                }

                timestampLastUpdate = DateTime.Now;


                CalculateCompassBearing();
            }
        }
		static void spatial_SpatialData(object sender, SpatialDataEventArgs e)
		{
			if (e.spatialData[0].Acceleration.Length > 0)
			{
				accx = e.spatialData[0].Acceleration[2]; 
				accy = e.spatialData[0].Acceleration[1]; 
				accz = e.spatialData[0].Acceleration[0]; 
			}
			double th = 0.05;
			
			if(accy > (-1)*th && accy < th)
			{
				Console.WriteLine(accy);
//				Console.WriteLine("Stable");
			}
			else
			{
				if(accy < (th * -1))
					servo.servos[0].Position -= 5;
				
				if(accy > th)
					servo.servos[0].Position += 5;
				
			}
			//			Console.WriteLine("{0} {1} {2}",accx,accy,accz);
//			Console.WriteLine("{0} {1} {2}",accy,accy,accy);
		}
Example #10
0
        void spatial_SpatialData(object sender, SpatialDataEventArgs e)
        {
            try
            {
                if (spatial.accelerometerAxes.Count > 0)
                {
                    xTextBox.Text = e.spatialData[0].Acceleration[0].ToString("F3");
                    yTextBox.Text = e.spatialData[0].Acceleration[1].ToString("F3");
                    zTextBox.Text = e.spatialData[0].Acceleration[2].ToString("F3");
                    drawAccelGraph();
                }

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

                    headingXText.Text = gyroHeading[0].ToString("F3") + "°";
                    headingYText.Text = gyroHeading[1].ToString("F3") + "°";
                    headingZText.Text = gyroHeading[2].ToString("F3") + "°";

                    drawGyroGraph();
                }

                //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)
                {
                    compassXTxt.Text = spatial.compassAxes[0].MagneticField.ToString("F3");
                    compassYTxt.Text = spatial.compassAxes[1].MagneticField.ToString("F3");
                    compassZTxt.Text = spatial.compassAxes[2].MagneticField.ToString("F3");

                    try
                    {
                        drawMagFieldGraph();
                        calculateCompassBearing();
                        drawCompassBearingGraph();
                    }
                    catch
                    {
                    }
                }
            }
            catch { }
        }
 //spatial data handler - all spatial data tied together.
 static void spatial_SpatialData(object sender, SpatialDataEventArgs e)
 {
     Console.WriteLine("SpatialData event time:" + e.spatialData[0].Timestamp.TotalSeconds.ToString());
     if (e.spatialData[0].Acceleration.Length > 0)
         Console.WriteLine(" Acceleration: " + e.spatialData[0].Acceleration[0] + ", " + e.spatialData[0].Acceleration[1] + ", " + e.spatialData[0].Acceleration[2]);
     if (e.spatialData[0].AngularRate.Length > 0)
         Console.WriteLine(" Angular Rate: " + e.spatialData[0].AngularRate[0] + ", " + e.spatialData[0].AngularRate[1] + ", " + e.spatialData[0].AngularRate[2]);
     if (e.spatialData[0].MagneticField.Length > 0)
         Console.WriteLine(" Magnetic Field: " + e.spatialData[0].MagneticField[0] + ", " + e.spatialData[0].MagneticField[1] + ", " + e.spatialData[0].MagneticField[2]);
 }
Example #12
0
        void phid_SpatialData(object sender, SpatialDataEventArgs e)
        {
            try
            {
                Pipeline.fillpen = Color.White;
                for (int i = 0; i < e.spatialData.Length; i++)
                {
                    if (zeroing)
                    {
                        if (e.spatialData[i].MagneticField.Length > 0)
                        {
                            magTemp.X += e.spatialData[i].MagneticField[0];
                            magTemp.Y += e.spatialData[i].MagneticField[2];
                            magTemp.Z -= e.spatialData[i].MagneticField[1];
                            magSamplesTaken++;
                        }

                        gravityTemp.X += e.spatialData[i].Acceleration[0];
                        gravityTemp.Y += e.spatialData[i].Acceleration[2];
                        gravityTemp.Z -= e.spatialData[i].Acceleration[1];
                        gravitySamplesTaken++;

                        if (gravitySamplesTaken >= gravitySamples)
                        {
                            gravityTemp.X /= gravitySamplesTaken;
                            gravityTemp.Y /= gravitySamplesTaken;
                            gravityTemp.Z /= gravitySamplesTaken;
                            gravityRef     = gravityTemp;

                            gravityTemp         = new Vector3(0, 0, 0);
                            gravitySamplesTaken = 0;

                            magTemp.X /= magSamplesTaken;
                            magTemp.Y /= magSamplesTaken;
                            magTemp.Z /= magSamplesTaken;
                            magRef     = new Vector3(magTemp.X, magTemp.Y, magTemp.Z);

                            magTemp         = new Vector3(0, 0, 0);
                            magSamplesTaken = 0;

                            zeroing = false;
                            finishZeroing();
                        }
                    }
                    else
                    {
                        if (lastMsCountGood)
                        {
                            double timechange        = e.spatialData[i].Timestamp.TotalMilliseconds - lastMsCount; // in ms
                            double timeChangeSeconds = (double)timechange / 1000.0;

                            calculateAttitude(e.spatialData[i], timeChangeSeconds);
                            calculatePosition(e.spatialData[i], timeChangeSeconds);
                        }
                    }

                    lastMsCount     = e.spatialData[i].Timestamp.TotalMilliseconds;
                    lastMsCountGood = true;

                    //sw.WriteLine(e.spatialData[i].Timestamp.TotalSeconds.ToString() + "," + e.spatialData[i].Acceleration[0] + "," + -e.spatialData[i].Acceleration[2] + "," + e.spatialData[i].Acceleration[1]);
                }
            }
            catch { }
        }