//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]); } }
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 { } }
// 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(); } }
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]); }
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 { } }
// 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); }
//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]); }
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 { } }