private void WriteLog(ref Axis x, ref Axis y, ref Axis z)
{
posLog.WriteLine(DateTime.Now.ToString() + "," + pos[posIndex][0].ToString() + "," + pos[posIndex][1].ToString() + "," + pos[posIndex][2].ToString()+","+ (x.angle).ToString() + "," + (y.angle).ToString() + "," + (z.angle).ToString());
}
private int GetMaxG(ref Axis s1, ref Axis s2, ref Axis s3)
{
if (Math.Abs(s1.accel[s1.accIndex]) >= Math.Abs(s2.accel[s2.accIndex]) && Math.Abs(s1.accel[s1.accIndex]) >= Math.Abs(s3.accel[s3.accIndex]))
return s1.index;
if (Math.Abs(s2.accel[s2.accIndex]) >= Math.Abs(s1.accel[s1.accIndex]) && Math.Abs(s2.accel[s2.accIndex]) >= Math.Abs(s3.accel[s3.accIndex]))
return s2.index;
if (Math.Abs(s3.accel[s3.accIndex]) >= Math.Abs(s1.accel[s1.accIndex]) && Math.Abs(s3.accel[s3.accIndex]) >= Math.Abs(s2.accel[s2.accIndex]))
return s3.index;
return -1;
}
private void UpdateTiltCompensation(ref Axis s1, ref Axis s2, ref Axis target)
{
if ((s1.state & 1) == 0 && (s2.state & 1) == 0)
{
double magX_prime = s2.mag * cos(s1.angle * Math.PI / 180.0f) + s1.mag * sin(s2.angle * Math.PI / 180.0f) * sin(s1.angle * Math.PI / 180.0f) - target.mag * cos(s2.angle * Math.PI / 180.0f) * sin(s1.angle * Math.PI / 180.0f);
double magZ_prime = s1.mag * cos(s2.angle * Math.PI / 180.0f) + target.mag * sin(s1.angle * Math.PI / 180.0f);
target.mcAngle = (float)(Math.Atan2(magZ_prime, magX_prime) * 180.0f / Math.PI);
// when use atan method
/*
if (magZ_prime < 0)
target.mcAngle = 180.0f - target.mcAngle;
else if (magZ_prime > 0 && magX_prime < 0)
target.mcAngle = -target.mcAngle;
else if (magZ_prime > 0 && magX_prime > 0)
target.mcAngle = 360.0f - target.mcAngle;
else if (magZ_prime == 0 && magX_prime < 0)
target.mcAngle = 90.0f;
else if (magZ_prime == 0 && magX_prime > 0)
target.mcAngle = 270.0f;
*/
}
}
private void UpdateYawAngle(ref Axis s1, ref Axis s2, ref Axis target)
{
if ((s1.state & 1)==0 && (s2.state & 1)==0 && (target.state & 1)==0)
{
int maxGIndex = GetMaxG(ref s1, ref s2, ref target);
switch(maxGIndex)
{
case 0:
case 2:
break;
case 1:
if (target.accel[target.accIndex] >= 0)
target.angle = target.mAngle;
else
target.angle = -target.mAngle;
break;
default:
break;
}
}
}
private void UpdateRollAngle(ref Axis s1, ref Axis s2, ref Axis target)
{
if ((s1.state & 1) == 0 && (s2.state & 1) == 0 && (target.state & 1) == 0)
{
int maxGIndex = GetMaxG(ref s1, ref s2, ref target);
switch(maxGIndex)
{
case 0:
break;
case 1:
case 2:
target.angle = (float)(Math.Atan2(Math.Sqrt(s1.accel[s1.accIndex]*s1.accel[s1.accIndex]+target.accel[target.accIndex]*target.accel[target.accIndex]), s2.accel[s2.accIndex]) * 180.0f / Math.PI);
target.angle -= 90.0f;
if (target.angle < -180.0f)
target.angle += 360.0f;
target.tiltAngle = target.angle;
if (s1.accel[s1.accIndex] < 0)
{
target.angle = 180.0f - target.angle;
if (target.angle > 180.0f)
target.angle -= 360.0f;
}
break;
default:
break;
}
}
return;
}
private void UpdateRotationMatrix(ref Axis x, ref Axis y, ref Axis z)
{
double xd = x.angle;
double yd = y.angle;
double zd = z.angle;
r[0][0] = (float)(cos(yd) * cos(zd));
r[0][1] = (float)(cos(zd) * sin(xd) * sin(yd) - cos(xd) * sin(zd));
r[0][2] = (float)(cos(xd) * cos(zd) * sin(yd) + sin(xd) * sin(zd));
r[1][0] = (float)(cos(yd) * sin(zd));
r[1][1] = (float)(cos(xd) * cos(zd) + sin(xd) * sin(yd) * sin(zd));
r[1][2] = (float)(cos(xd) * sin(yd) * sin(zd) - cos(zd) * sin(xd));
r[2][0] = (float)(-sin(yd));
r[2][1] = (float)(cos(yd) * sin(xd));
r[2][2] = (float)(cos(xd) * cos(yd));
/*
r[0][0] = 1.0f;
r[0][1] = 0.0f;
r[0][2] = 0.0f;
r[1][0] = 0.0f;
r[1][1] = 1.0f;
r[1][2] = 0.0f;
r[2][0] = 0.0f;
r[2][1] = 0.0f;
r[2][2] = 1.0f;
*/
}
private void UpdateMovement(ref Axis x, ref Axis y, ref Axis z)
{
if((x.state & 1)==0)
vw[vwIndex][0] = 0.0f;
if((y.state & 1) == 0)
vw[vwIndex][1] = 0.0f;
if((z.state & 1)==0)
vw[vwIndex][2] = 0.0f;
if ((x.state & 1) == 0 && (y.state & 1) == 0 && (z.state & 1) == 0)
{
if (zuptFlag == false)
{
posLog.WriteLine("ZUPTs,0,0,0");
zuptFlag = true;
}
return;
}
else
zuptFlag = false;
int beforePosIndex = posIndex;
posIndex++;
if (posIndex >= 2)
posIndex = 0;
for (int i = 0; i < 3; i++)
{
switch (vwIndex)
{
case 0:
pos[posIndex][i] = pos[beforePosIndex][i] + (vw[0][i] + 2 * vw[1][i] + 2 * vw[2][i] + vw[3][i]) * MOTIONNODE_FREQ / 6;
break;
case 1:
pos[posIndex][i] = pos[beforePosIndex][i] + (vw[1][i] + 2 * vw[2][i] + 2 * vw[3][i] + vw[0][i]) * MOTIONNODE_FREQ / 6;
break;
case 2:
pos[posIndex][i] = pos[beforePosIndex][i] + (vw[2][i] + 2 * vw[3][i] + 2 * vw[0][i] + vw[1][i]) * MOTIONNODE_FREQ / 6;
break;
case 3:
pos[posIndex][i] = pos[beforePosIndex][i] + (vw[3][i] + 2 * vw[0][i] + 2 * vw[1][i] + vw[2][i]) * MOTIONNODE_FREQ / 6;
break;
}
}
WriteLog(ref x, ref y, ref z);
}
private void UpdatePitchAngle(ref Axis s1, ref Axis s2, ref Axis target)
{
if ((s1.state & 1) == 0 && (s2.state & 1) == 0 && (target.state & 1) == 0)
{
int maxGIndex = GetMaxG(ref s1, ref s2, ref target);
switch(maxGIndex)
{
case 0:
case 1:
/*
if (s1.accel[s1.accIndex] == 0.0f)
{
if (s2.accel[s2.accIndex] >= 0)
target.angle = 90.0f;
else
target.angle = -90.0f;
}
else
{
target.angle = (float)(Math.Atan2(s1.accel[s1.accIndex], s2.accel[s2.accIndex]) * 180.0f / Math.PI);
}
*/
target.angle = (float)(Math.Atan2(s1.accel[s1.accIndex], Math.Sqrt(s2.accel[s2.accIndex]*s2.accel[s2.accIndex]+target.accel[target.accIndex]*target.accel[target.accIndex])) * 180.0f / Math.PI);
break;
case 2:
break;
default:
break;
}
}
return;
}
private bool UpdateGyroAngleInTilt(ref Axis s, ref Axis ds)
{
float temp = 0.0f;
switch (s.gyroIndex)
{
case 0:
temp += (s.gyro[0] + 2 * s.gyro[1] + 2 * s.gyro[2] + s.gyro[3]) * MOTIONNODE_FREQ / 6;
break;
case 1:
temp += (s.gyro[1] + 2 * s.gyro[2] + 2 * s.gyro[3] + s.gyro[0]) * MOTIONNODE_FREQ / 6;
break;
case 2:
temp += (s.gyro[2] + 2 * s.gyro[3] + 2 * s.gyro[0] + s.gyro[1]) * MOTIONNODE_FREQ / 6;
break;
case 3:
temp += (s.gyro[3] + 2 * s.gyro[0] + 2 * s.gyro[1] + s.gyro[2]) * MOTIONNODE_FREQ / 6;
break;
}
if (ds.accel[ds.accIndex] >= 0)
s.tiltAngle += temp * 2.0f;
else
s.tiltAngle -= temp * 2.0f;
if (s.angle > 180.0f)
s.angle -= 360.0f;
else if (s.angle < -180.0f)
s.angle += 360.0f;
return true;
}
private bool UpdateMagneticAngle(ref Axis s1, ref Axis s2, ref Axis target)
{
if((s1.state & 1) == 0 && (s2.state & 1)==0)
{
if (s1.mag == 0.0f)
target.mAngle = 0.0f;
else
{
//target.mAngle = (float)(Math.Atan(s2.mag / s1.mag) * 180.0f / Math.PI);
target.mAngle = Math.Abs((float)(Math.Atan(s2.mag / s1.mag) * 180.0f / Math.PI));
if (s1.mag > 0)
target.mAngle = 180.0f - target.mAngle;
if (s2.mag < 0)
target.mAngle = -(target.mAngle);
}
target.state |= (1 << 3);
return true;
}
target.state &= ~(1 << 3);
return false;
}
private void UpdateEulerAngle(ref Axis x, ref Axis y, ref Axis z)
{
if((x.state & 1)==0 && (y.state & 1)==0 && (z.state & 1) == 0)
{
x.angle = x.eulerAngle;
y.angle = y.eulerAngle;
z.angle = z.eulerAngle;
}
return;
}
private void UpdateAccelVector(ref Axis x, ref Axis y, ref Axis z)
{
al[0] = x.accel[x.accIndex] * G_VALUE;
al[1] = y.accel[y.accIndex] * G_VALUE;
al[2] = z.accel[z.accIndex] * G_VALUE;
awIndex++;
if (awIndex >= 4)
awIndex = 0;
aw[awIndex][0] = 0.0f;
aw[awIndex][1] = 0.0f;
aw[awIndex][2] = 0.0f;
for(int i=0 ; i < 3 ; i++)
{
aw[awIndex][i] = 0.0f;
for(int j=0 ; j < 3 ; j++)
{
aw[awIndex][i] += al[j] * r[j][i] ;
}
}
aw[awIndex][1] -= G_VALUE;
//this.Invoke(new MethodInvoker(delegate()
//{
// this.GyroXData.Text = aw[awIndex][0].ToString();
// this.GyroYData.Text = aw[awIndex][1].ToString();
// this.GyroZData.Text = aw[awIndex][2].ToString();
//}));
for(int i=0; i < 3 ; i++)
awSum[i] -= awHistory[awSumIndex][i];
for(int i=0 ; i < 3 ; i++)
awHistory[awSumIndex][i] = aw[awIndex][i];
for (int i = 0; i < 3; i++)
awSum[i] += awHistory[awSumIndex][i];
awSumIndex++;
if (awSumIndex >= awSize)
awSumIndex = 0;
if ((x.state & 1) == 0 && (y.state & 1) == 0 && (z.state & 1) == 0)
{
for(int i=0 ; i < 3; i++)
{
awZero[i] = awSum[i] / awSize;
}
}
//this.Invoke(new MethodInvoker(delegate()
//{
// this.AngularXData.Text = awZero[0].ToString();
// this.AngularYData.Text = awZero[1].ToString();
// this.AngularZData.Text = awZero[2].ToString();
//}));
for (int i = 0; i < 3; i++)
aw[awIndex][i] -= awZero[i];
//this.Invoke(new MethodInvoker(delegate()
//{
// this.MagnetoXData.Text = aw[awIndex][0].ToString();
// this.MagnetoYData.Text = aw[awIndex][1].ToString();
// this.MagnetoZData.Text = aw[awIndex][2].ToString();
//}));
int beforeVwIndex = vwIndex;
vwIndex++;
if (vwIndex >= 4)
vwIndex = 0;
for (int i = 0; i < 3; i++)
{
switch (awIndex)
{
case 0:
vw[vwIndex][i] = vw[beforeVwIndex][i] + (aw[0][i] + 2 * aw[1][i] + 2 * aw[2][i] + aw[3][i]) * MOTIONNODE_FREQ / 6;
break;
case 1:
vw[vwIndex][i] = vw[beforeVwIndex][i] + (aw[1][i] + 2 * aw[2][i] + 2 * aw[3][i] + aw[0][i]) * MOTIONNODE_FREQ / 6;
break;
case 2:
vw[vwIndex][i] = vw[beforeVwIndex][i] + (aw[2][i] + 2 * aw[3][i] + 2 * aw[0][i] + aw[1][i]) * MOTIONNODE_FREQ / 6;
break;
case 3:
vw[vwIndex][i] = vw[beforeVwIndex][i] + (aw[3][i] + 2 * aw[0][i] + 2 * aw[1][i] + aw[2][i]) * MOTIONNODE_FREQ / 6;
break;
}
}
}
private bool UpdateAccelAngle(ref Axis s1, ref Axis s2, ref Axis target)
{
if ((s1.state & 1) == 0 && (s2.state & 1) == 0)
{
if (s1.accel[s1.accIndex] == 0.0f)
{
if (s2.accel[s2.accIndex] >= 0)
target.angle = 90.0f;
else
target.angle = -90.0f;
}
else
{
target.angle = Math.Abs((float)(Math.Atan(s2.accel[s2.accIndex] / s1.accel[s1.accIndex]) * 180.0f / Math.PI));
if (s1.accel[s1.accIndex] >= 0)
target.angle = 180.0f - target.angle;
if (s2.accel[s2.accIndex] <= 0)
target.angle = -target.angle;
}
if(target.index == 0)
{
target.angle += 180.0f;
if (target.angle > 180.0f)
target.angle -= 360.0f;
if(target.mag < 0)
{
target.angle -= 180.0f;
if (target.angle < -180.0f)
target.angle += 360.0f;
}
}
if (target.index == 2)
{
target.angle -= 90.0f;
if (target.angle < -180.0f)
target.angle += 360.0f;
}
target.state |= (1 << 2);
return true;
}
target.state &= ~(1 << 2);
return false;
}
public void updateWeight(ref Axis s1, ref Axis s2)
{
if(s1.stdev + s2.stdev + gyroStdev == 0)
{
weightGyro = 0.0f;
}
else
{
weightGyro = gyroStdev / (s1.stdev + s2.stdev);
if (weightGyro < 0)
weightGyro = 0.0f;
else if (weightGyro > 1)
weightGyro = 1.0f;
}
if(s1.gyroStdev + s2.gyroStdev + stdev == 0)
{
weightAccel = 0.0f;
}
else
{
weightAccel = stdev / (s1.gyroStdev + s2.gyroStdev + stdev);
if (weightAccel < 0)
weightAccel = 0.0f;
else if (weightAccel > 1)
weightAccel = 1.0f;
}
}
public void updateAccelWeight(ref Axis s1)
{
if (stdev + s1.stdev == 0)
weightAccel = 0.0f;
else
{
weightAccel = stdev / (stdev + s1.stdev);
if (weightAccel < 0)
weightAccel = 0.0f;
else if (weightAccel > 1)
weightAccel = 1.0f;
}
}
