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 );
}
