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