private void formUpdateTimer_Tick(object sender, EventArgs e) { //sp.BaseStream.WriteByte(5); sp.BaseStream.WriteByte(5); string line = sp.ReadLine(); string[] sl = line.Split(';').ToArray(); //float accX = float.Parse(sl[0]) - 460f; //float accY = float.Parse(sl[1])-260f; //float accZ = float.Parse(sl[2])+16590f; float accX = float.Parse(sl[0]); float accY = float.Parse(sl[1]); float accZ = float.Parse(sl[2]); //accX *= (4f / 65536f); //accY *= (4f / 65536f); //accZ *= (4f / 65536f); float gyroX = float.Parse(sl[4]); float gyroY = float.Parse(sl[5]); float gyroZ = float.Parse(sl[6]); //gyroX -= 58f; //gyroY -= 450.0f + 170f; //gyroZ -= 53f; //accX = 0; //accY = 500; //accZ = 0; //gyroX = 0; //gyroY = 0; //gyroZ = 0; lock (this) { //AHRS.Update(0.52f, 0.52f, 0.52f, 0.0f, 0.1f, 0.0f); AHRS.Update(deg2rad(gyroX), deg2rad(gyroY), deg2rad(gyroZ), accX, accY, accZ); RotationMatrix = (new x_IMU_API.QuaternionData(AHRS.Quaternion)).ConvertToConjugate().ConvertToRotationMatrix(); float pitch, roll, yaw; float[] q = AHRS.Quaternion; roll = (float)Math.Atan((2 * (q[0] * q[1] + q[2] * q[3])) / (1 - 2 * (q[1] * q[1] + q[2] * q[2]))); pitch = (float)Math.Asin(2 * (q[0] * q[2] - q[3] * q[1])); yaw = (float)Math.Atan((2 * (q[0] * q[3] + q[1] * q[2])) / (1 - 2 * (q[2] * q[2] + q[3] * q[3]))); yaw *= (float)(180.0 / Math.PI); pitch *= (float)(180.0 / Math.PI); roll *= (float)(180.0 / Math.PI); Console.WriteLine("yaw: {0}\tpitch: {1}\troll: {2}", yaw.ToString("f1"), pitch.ToString("f1"), roll.ToString("f1")); simpleOpenGlControl.Refresh(); } }
private void filterTimer_Tick(object sender, EventArgs e) { if (!float.IsNaN(mx) && !float.IsNaN(my) && !float.IsNaN(mz)) { AHRSFilter.Update(deg2rad(gx), deg2rad(gy), deg2rad(gz), ax, ay, az, my, mx, -mz); float[] q = AHRSFilter.Quaternion; raw_quat = new Quaternion(q[1], q[2], q[3], q[0]); // Apply origin offset Quaternion delta_quat_orig = raw_quat * Quaternion.Inverse(quat_orig); quat_trans = Quaternion.Inverse(quat_orig) * delta_quat_orig * quat_orig; QuatToEuler(quat_trans, ref yaw, ref pitch, ref roll); } }
//// in playback mode - waits until the given time stamp //private void WaitForPlaybackTimestamp(string frameSource, long frameTimestamp) //{ // long currentPlayTime = DateTime.Now.Ticks - playbackStartTime; // if (currentPlayTime < frameTimestamp) // { // Debug.Log("Waiting for " + frameSource + " timestamp " + frameTimestamp + ". Current play time is: " + currentPlayTime); // } // while (currentPlayTime < frameTimestamp) // { // currentPlayTime = DateTime.Now.Ticks - playbackStartTime; // } //} // processes the IMU frame private void ProcessImuFrame(ImuSample imuSample) { if (kinectPlayback != null) { //WaitForPlaybackTimestamp("acc", imuSample.AccelerometerTimestampInUsec); //WaitForPlaybackTimestamp("gyro", imuSample.GyroTimestampInUsec); } lastImuSample = curImuSample; curImuSample = imuSample; if (!flipImuRotation && lastImuSample == null && Mathf.Abs(imuSample.AccelerometerSample.Y) >= 1f) { // check for rolled imu flipImuRotation = true; imuTurnRot2 = Quaternion.Euler(90f, -90f, 0f); } if (lastImuSample != null && imuAhrs != null && imuSample.AccelerometerTimestamp.Ticks != lastImuSample.AccelerometerTimestamp.Ticks) { prevRotZ = imuRotation.eulerAngles.z; Array.Copy(imuAhrs.Quaternion, prevQuat, prevQuat.Length); imuAhrs.SamplePeriod = ((imuSample.AccelerometerTimestamp.Ticks - lastImuSample.AccelerometerTimestamp.Ticks) / 10) * 0.000001f; imuAhrs.Update(imuSample.GyroSample.X, imuSample.GyroSample.Y, imuSample.GyroSample.Z, imuSample.AccelerometerSample.X, imuSample.AccelerometerSample.Y, imuSample.AccelerometerSample.Z); float[] ahrsQuat = imuAhrs.Quaternion; imuRotation = new Quaternion(ahrsQuat[1], ahrsQuat[2], ahrsQuat[3], ahrsQuat[0]); Quaternion transRotation = imuTurnRot2 * imuRotation * imuTurnRot1; if (imuAhrs.E2 < 0.0001f && Mathf.Abs(imuRotation.eulerAngles.z - prevRotZ) <= 0.1f) { // correct the continuous yaw, when imu doesn't move Array.Copy(prevQuat, imuAhrs.Quaternion, prevQuat.Length); ahrsQuat = prevQuat; imuRotation = new Quaternion(ahrsQuat[1], ahrsQuat[2], ahrsQuat[3], ahrsQuat[0]); transRotation = imuTurnRot2 * imuRotation * imuTurnRot1; if (!imuYawAtStartSet) { // initial yaw imuYawAtStartSet = true; imuYawAtStart = new Vector3(0f, transRotation.eulerAngles.y, 0f); imuFixAtStart = Quaternion.Inverse(Quaternion.Euler(imuYawAtStart)); } } if (imuYawAtStartSet) { // fix the initial yaw transRotation = imuFixAtStart * transRotation; } lock (poseFrameLock) { // rawPosePosition rawPoseRotation = transRotation; rawPoseTimestamp = (ulong)imuSample.AccelerometerTimestamp.Ticks; //poseFrameNumber = currentFrameNumber; } } //var onImuSample = OnImuSample; //if(onImuSample != null) //{ // onImuSample.Invoke(imuSample, rawDepthTimestamp, rawColorTimestamp); //} }
private static void Processing1() { while (true) { lock (data1) { while (data1.Count < 50) { Monitor.Wait(data1); } int i = 1; while (i == 1) { RxPkt1[0] = data1.Dequeue(); if (RxPkt1[0] == 16) { RxPkt1[1] = data1.Dequeue(); if (RxPkt1[1] == 1) { int j = 1; int k = 2; while (j == 1) { RxPkt1[k] = data1.Dequeue(); if (RxPkt1[k] == 16) { RxPkt1[k + 1] = data1.Dequeue(); if (RxPkt1[k + 1] == 4) { j = 0; i = 0; } } k++; } } } } byte[] convert = new byte[26]; for (i = 0; i < 26; i++) { convert[i] = RxPkt1[i]; } Array.Reverse(convert);//operating system is little endians while the package is big endians, reverse the array. float[] Accelerometer = new float[3]; float[] Gyroscope = new float[3]; float[] Mag = new float[3]; Accelerometer[0] = (float)(BitConverter.ToInt16(convert, 22)) / 4096; Accelerometer[1] = (float)(BitConverter.ToInt16(convert, 20)) / 4096; Accelerometer[2] = (float)(BitConverter.ToInt16(convert, 18)) / 4096; Gyroscope[0] = (float)(BitConverter.ToInt16(convert, 16)) / (float)32.75; Gyroscope[1] = (float)(BitConverter.ToInt16(convert, 14)) / (float)32.75; Gyroscope[2] = (float)(BitConverter.ToInt16(convert, 12)) / (float)32.75; if (Gyroscope[0] < 3 && Gyroscope[0] > -3) { Gyroscope[0] = 0; } if (Gyroscope[1] < 3 && Gyroscope[1] > -3) { Gyroscope[1] = 0; } if (Gyroscope[2] < 3 && Gyroscope[2] > -3) { Gyroscope[2] = 0; } thisRTime = toMs(RxPkt1[23], RxPkt1[24], timerOffsetFromOverflow); if (thisRTime < lastRTime) { overflowsTime += lastRTime; timerOffsetFromOverflow = 65536 - lastTimerValue; offsetCombined = (uint)((RxPkt1[23] << 8) + RxPkt1[24] + timerOffsetFromOverflow); thisRTime = toMs(RxPkt1[23], RxPkt1[24], timerOffsetFromOverflow); } lastRTime = thisRTime; lastTimerValue = (uint)((RxPkt1[23] << 8) + RxPkt1[24]); time1 = (thisRTime + overflowsTime - firstRTime) / 1000; form_3DcuboidA.RotationMatrix = ConvertToRotateMatrix(AHRS1.Quaternion); form_3DcuboidB.RotationMatrix = ConvertToRotateMatrix(AHRS.Quaternion); // these two lines call the filtering algorithm. AHRS1.Update(-deg2rad(Gyroscope[0]), deg2rad(Gyroscope[1]), -deg2rad(Gyroscope[2]), -Accelerometer[0], Accelerometer[1], -Accelerometer[2], time1); AHRS.Update(-deg2rad(Gyroscope[0]), deg2rad(Gyroscope[1]), -deg2rad(Gyroscope[2]), -Accelerometer[0], Accelerometer[1], -Accelerometer[2], time1); } } }
//*********************************************** // // data proccessing //********************************************** public void Data_proccessing(int n) { /* if (s.Length == 48) * { * * Int32.TryParse(s.Substring(1, 3), out MAG[0]); * Int32.TryParse(s.Substring(5, 3), out MAG[1]); * Int32.TryParse(s.Substring(9, 3), out MAG[2]); * * UInt32.TryParse(s.Substring(12, 4), out ACC[0]); * UInt32.TryParse(s.Substring(16, 4), out ACC[1]); * UInt32.TryParse(s.Substring(20, 4), out ACC[2]); * * UInt32.TryParse(s.Substring(24, 4), out GYRO[0]); * UInt32.TryParse(s.Substring(28, 4), out GYRO[1]); * UInt32.TryParse(s.Substring(32, 4), out GYRO[2]); * * UInt32.TryParse(s.Substring(36, 4), out ENCODER[0]); * UInt32.TryParse(s.Substring(40, 4), out ENCODER[1]); * UInt32.TryParse(s.Substring(44, 4), out ENCODER[2]); * * if (s.Substring(0, 1) == "-") * { * MAG[0] = -MAG[0]; * } * if (s.Substring(4, 1) == "-") * { * MAG[1] = -MAG[1]; * } * if (s.Substring(8, 1) == "-") * { * MAG[2] = -MAG[2]; * } * }*/ MAG[0] = (Int32)((Input[1] - 0x30) * 100 + (Input[2] - 0x30) * 10 + (Input[3] - 0x30)); MAG[1] = (Int32)((Input[5] - 0x30) * 100 + (Input[6] - 0x30) * 10 + (Input[7] - 0x30)); MAG[2] = (Int32)((Input[9] - 0x30) * 100 + (Input[10] - 0x30) * 10 + (Input[11] - 0x30)); ACC[0] = (UInt32)((Input[12] - 0x30) * 1000 + (Input[13] - 0x30) * 100 + (Input[14] - 0x30) * 10 + (Input[15] - 0x30)); ACC[1] = (UInt32)((Input[16] - 0x30) * 1000 + (Input[17] - 0x30) * 100 + (Input[18] - 0x30) * 10 + (Input[19] - 0x30)); ACC[2] = (UInt32)((Input[20] - 0x30) * 1000 + (Input[21] - 0x30) * 100 + (Input[22] - 0x30) * 10 + (Input[23] - 0x30)); GYRO[0] = (UInt32)((Input[24] - 0x30) * 1000 + (Input[25] - 0x30) * 100 + (Input[26] - 0x30) * 10 + (Input[27] - 0x30)); GYRO[1] = (UInt32)((Input[28] - 0x30) * 1000 + (Input[29] - 0x30) * 100 + (Input[30] - 0x30) * 10 + (Input[31] - 0x30)); GYRO[2] = (UInt32)((Input[32] - 0x30) * 1000 + (Input[33] - 0x30) * 100 + (Input[34] - 0x30) * 10 + (Input[35] - 0x30)); ENCODER[0] = (UInt32)((Input[36] - 0x30) * 1000 + (Input[37] - 0x30) * 100 + (Input[38] - 0x30) * 10 + (Input[39] - 0x30)); ENCODER[1] = (UInt32)((Input[40] - 0x30) * 1000 + (Input[41] - 0x30) * 100 + (Input[42] - 0x30) * 10 + (Input[43] - 0x30)); ENCODER[2] = (UInt32)((Input[44] - 0x30) * 1000 + (Input[45] - 0x30) * 100 + (Input[46] - 0x30) * 10 + (Input[47] - 0x30)); if (Input[0] == '-') { MAG[0] = -MAG[0]; } if (Input[4] == '-') { MAG[1] = -MAG[1]; } if (Input[8] == '-') { MAG[2] = -MAG[2]; } // } //calculate to real value for filter int a = MAG[0]; MAG[0] = -MAG[1]; MAG[1] = a; MAG[2] = MAG[2]; this.form_3DcuboidB.X = (float)MAG[0]; this.form_3DcuboidB.Y = (float)MAG[1]; this.form_3DcuboidB.Z = (float)MAG[2]; ax = ((float)ACC[0] - (float)ACC_OFFSET[0]) * Acc_sensity; // ax real ay = ((float)ACC[1] - (float)ACC_OFFSET[1]) * Acc_sensity; // ay real az = ((float)ACC[2] - (float)ACC_OFFSET[2]) * Acc_sensity; gx = -((float)GYRO[0] - (float)GYRO_OFFSET[0]) * Gyro_sensity; gy = ((float)GYRO[1] - (float)GYRO_OFFSET[1]) * Gyro_sensity; gz = ((float)GYRO[2] - (float)GYRO_OFFSET[2]) * Gyro_sensity; // float norm = (float)Math.Sqrt ( ax * ax + ay * ay + az * az ); // ax /= norm; // ay /= norm; // az /= norm; // // // En_Angle[1] = (float)((ENCODER[0] / 4095.0f) * 2.0f * Math.PI); En_Angle[0] = (float)((ENCODER[1] / 4095.0f) * 2.0f * Math.PI); En_Angle[2] = (float)((ENCODER[2] / (6395.0f)) * 2.0f * Math.PI); if (En_Angle[0] > Math.PI) { En_Angle[0] -= (float)Math.PI * 2; } if (En_Angle[1] > Math.PI) { En_Angle[1] -= (float)Math.PI * 2; } if (En_Angle[2] > Math.PI) { En_Angle[2] -= (float)Math.PI * 2; } /* if(( En_Angle[0]==0)&& (Pre_En_Angle[0]==0)) * { * En_Angle[0]=0; * } * if( ( En_Angle[0]==0) & (Pre_En_Angle[0] != 0)) * { * En_Angle[0]=Pre_En_Angle[0]; * } * if (( En_Angle[0] != 0 )) * { * Pre_En_Angle[0] = En_Angle[0]; * } * if (( En_Angle[1] == 0 ) & ( Pre_En_Angle[1] == 0 )) * { * En_Angle[1] = 0; * } * if (( En_Angle[1] == 0 ) & ( Pre_En_Angle[1] != 0 )) * { * En_Angle[1] = Pre_En_Angle[1]; * } * if (( En_Angle[1] != 0 ) ) * { * Pre_En_Angle[1] = En_Angle[1]; * }*/ // // //this for Encoder // // En_Angle[0] = 1; // En_Angle[1] = 1; // En_Angle[2] = 1; // float[] Quatemp = MyQuaternion.getQuaternionFromAngles3(En_Angle); //En_Angle = MyQuaternion.getAnglesFromQuaternion2(Quatemp); this.form_3DcuboidA.RotationMatrix = MyQuaternion.getRotaionMatrixFromAngle(En_Angle); // this.form_3DcuboidA.RotationMatrix = MyQuaternion.getRotaionMatrixFromAngle ( En_Angle ); //MatrixLibrary.Matrix mat = new MatrixLibrary.Matrix(3,3); //mat[0,0] = rotation[0]; // mat[0, 1] = rotation[1]; //mat[0, 2] = rotation[2]; //mat[1, 0] = rotation[3]; //mat[1, 1] = rotation[4]; //mat[1, 2] = rotation[5]; //mat[2, 0] = rotation[6]; //mat[2, 1] = rotation[7]; //mat[2, 2] = rotation[8]; //double[] dtemp = MyQuaternion.getAngleFromRotation(mat); //float[] ftemp = new float[3]; //ftemp[0] = (float)dtemp[0]; //ftemp[1] = (float)dtemp[1]; //ftemp[2] = (float)dtemp[2]; //this.form_3DcuboidC.RotationMatrix = MyQuaternion.getRotaionMatrixFromAngle ( ftemp ); //gx =(float) HPfilterp.step ((double) gx ); // 16-bit uint ADC gyrocope values to rad/s (HP filtered) //gy = (float)HPfilterp.step ( (double)gy); //gz = (float)HPfilterp.step ( (double)gz); // //update // // AHRS.Update(gx,gy,gz,ax,ay,az); // Quaternion = AHRS.Quaternion; // this.form_3DcuboidA.RotationMatrix = MyQuaternion.getRotationMatrixFromQuaternion3 ( Quatemp ); // //send data to 3D form // this.form_3DcuboidB.Accx = (float)ax; this.form_3DcuboidB.Accy = (float)ay; this.form_3DcuboidB.Accz = (float)az; this.form_3DcuboidB.Gyrox = (float)gx; this.form_3DcuboidB.Gyroy = (float)gy; this.form_3DcuboidB.Gyroz = (float)gz; // //for accellero only // AHRS.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az); if (n == 1) { Kalman.filterStep((double)gx * 180.00f / Math.PI, (double)gy * 180.00f / Math.PI, (double)gz * 180.00f / Math.PI, (double)ax, (double)ay, (double)az, (double)MAG[0], (double)MAG[1], (double)MAG[2]); // Kalman.filterStep((double)gx , (double)gy, (double)gz, (double)ax, (double)ay, (double)az, (double)MAG[0], (double)MAG[1], (double)MAG[2]); Quaternion[0] = Kalman.q_filt1; Quaternion[1] = Kalman.q_filt2; Quaternion[2] = Kalman.q_filt3; Quaternion[3] = Kalman.q_filt4; } else { AHRS.Update((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az, (float)MAG[0], (float)MAG[1], (float)MAG[2]);// Quaternion = AHRS.Quaternion; } // Quaternion[0] = 1.0f; // Quaternion[1] = 0.5f; // Quaternion[2] = 0.0f; //Quaternion[3] = 0.0f; //float[] Angle3 = MyQuaternion.getAnglesFromQuaternion2(Quaternion); //this.form_3DcuboidB.X = (float)Angle3[0]; //this.form_3DcuboidB.Y = (float)Angle3[1]; //this.form_3DcuboidB.Z = (float)Angle3[2]; // float[] temp1 = MyQuaternion.getRotationMatrixFromQuaternion3(Quaternion); // MatrixLibrary.Matrix mat = new MatrixLibrary.Matrix ( 3, 3 ); float[] temp1 = MyQuaternion.getRotationMatrixFromQuaternion3(Quaternion); float[] dtemp = MyQuaternion.getAnglesFromQuaternion2(Quaternion); /* mat[0, 0] = temp1[0]; * mat[0, 1] = temp1[1]; * mat[0, 2] = temp1[2]; * mat[1, 0] = temp1[3]; * mat[1, 1] = temp1[4]; * mat[1, 2] = temp1[5]; * mat[2, 0] = temp1[6]; * mat[2, 1] = temp1[7]; * mat[2, 2] = temp1[8];*/ // double[] dtemp = MyQuaternion.getAngleFromRotation ( mat ); //m double[] Angle5 = new double[3]; //m Angle5[0] = En_Angle2[0]; //m Angle5[1] = En_Angle2[1]; //m Angle5[2] = dtemp[2]; // dtemp[2] = En_Angle[2]; // this.form_3DcuboidB.Gyrox = (float)(dtemp[0] * 180 / Math.PI) ;//Angle5[0]; // this.form_3DcuboidB.Gyroy = (float)(dtemp[1] * 180 / Math.PI);//Angle5[1]; // this.form_3DcuboidB.Gyroz = (float)(dtemp[2] * 180 / Math.PI);//]Angle5[2]; // float[] temp3 = MyQuaternion.getRotaionMatrixFromAngle2(dtemp);//Angle5 this.form_3DcuboidB.RotationMatrix = temp1; // //Error angle // /*m ERROR[0] = (En_Angle[0] -(float)Angle5[0])*180 / (float)Math.PI; * ERROR[1] = En_Angle[1] - (float)Angle5[1] * 180 / (float)Math.PI;*/ // ERROR[0] = (float)(dtemp[0] * 180 / Math.PI); ; //ERROR[1] = (float)(En_Angle[0] * 180 / Math.PI); // ERROR[2] = (float)(dtemp[0] * 180 / Math.PI); //ERROR[3] = 0; // LIST[0] = (float)(dtemp[0] * 180 / Math.PI); LIST[1] = (float)(En_Angle[0] * 180 / Math.PI); LIST[2] = (float)(dtemp[1] * 180 / Math.PI); LIST[3] = (float)(En_Angle[1] * 180 / Math.PI); LIST[4] = (float)(dtemp[2] * 180 / Math.PI); LIST[5] = (float)(En_Angle[2] * 180 / Math.PI); plotForm.add_graph = LIST; // // // // /*MatrixLibrary.Matrix Quaternion12 = new MatrixLibrary.Matrix ( 4, 1 ); * MatrixLibrary.Matrix Angle4 = new MatrixLibrary.Matrix ( 3, 1 ); * Quaternion12[0,0] = 1/Math.Sqrt(4); * Quaternion12[1, 0] =- 1 / Math.Sqrt ( 4); * Quaternion12[2, 0] = -1 / Math.Sqrt ( 4 ); ; * Quaternion12[3, 0] = -1 / Math.Sqrt ( 4 ); * * Angle4 = MyQuaternion.getAnglesFromQuaternion ( Quaternion12 ); * MatrixLibrary.Matrix Temp_AN = new MatrixLibrary.Matrix ( 3, 1 ); * MatrixLibrary.Matrix QuaRe = new MatrixLibrary.Matrix ( 4, 1 ); * QuaRe = MyQuaternion.getQuaternionFromAngles2 ( Angle4 ); * QuaRe = MyQuaternion.getQuaternionFromAngles ( Angle4 ); * */ //this.form_3DcuboidB.RotationMatrix = temp1; //QuaRe = MyQuaternion.getQuaternionFromAngles ( Angle4 ); }