public void mUpdateAngle(ref Axis hor, ref Axis ver)
{
int b = mAccIndex - 1;
if(b < 0)
b = mSize - 1;
int b_ver = ver.mAccIndex - 1;
if(b_ver < 0)
b_ver = ver.mSize - 1;
int b_hor = hor.mAccIndex - 1;
if(b_hor < 0)
b_hor = hor.mSize - 1;
mAngle = (float)(Math.Atan2(mAcc[b],Math.Sqrt(ver.mAcc[b_ver]*ver.mAcc[b_ver]+hor.mAcc[b_hor]*hor.mAcc[b_hor])));
}
private void mUpdateMovingTiltHeading(ref Axis x, ref Axis y, ref Axis z)
{
float yval = y.mGetMag();
if (yval < -25.0f)
yval = -25.0f;
else if (yval > 25.0f)
yval = 25.0f;
float xval = x.mGetMag();
if (xval < -25.0f)
xval = -25.0f;
else if (xval > 25.0f)
xval = 25.0f;
float zval = x.mGetMag();
if (zval < -25.0f)
zval = -25.0f;
else if (zval > 25.0f)
zval = 25.0f;
double x_prime = xval * Math.Cos(x.mGetAngle()) + yval * Math.Sin(y.mGetAngle()) * Math.Sin(x.mGetAngle()) + zval * Math.Cos(y.mGetAngle()) * Math.Sin(x.mGetAngle());
double y_prime = yval * Math.Cos(y.mGetAngle()) - zval * Math.Sin(x.mGetAngle());
float cur = (float)(Math.Atan2(y_prime, x_prime));
mMovingTiltHeadingSum += cur;
mMovingTiltHeadingAvg = mMovingTiltHeadingSum / movingCount;
mMovingTiltHeadingAvg = mMovingTiltHeadingAvg - (float)(20.0f * Math.PI / 180.0f);
if (mMovingTiltHeadingAvg < -Math.PI)
mMovingTiltHeadingAvg += (float)(2 * Math.PI);
mStepCountMovingTiltHeadingAvg = mMovingTiltHeadingAvg;
mStepCountMovingTiltHeadingAvg = mStepCountMovingTiltHeadingAvg - (float)(90.0f * Math.PI / 180.0f);
if (mStepCountMovingTiltHeadingAvg < -Math.PI)
mStepCountMovingTiltHeadingAvg += (float)(2 * Math.PI);
}
private void mUpdateTiltHeading(ref Axis x, ref Axis y, ref Axis z)
{
float yval = y.mGetMag();
float xval = x.mGetMag();
float zval = x.mGetMag();
//double x_prime = x.mGetMag() * Math.Cos(x.mGetAngle()) + y.mGetMag() * Math.Sin(y.mGetAngle()) * Math.Sin(x.mGetAngle()) + z.mGetMag() * Math.Cos(y.mGetAngle()) * Math.Sin(x.mGetAngle());
//double y_prime = y.mGetMag() * Math.Cos(y.mGetAngle()) - z.mGetMag() * Math.Sin(x.mGetAngle());
double x_prime_s = xval * Math.Cos(x.mGetAngle()) + yval * Math.Sin(y.mGetAngle()) * Math.Sin(x.mGetAngle()) - zval * Math.Cos(y.mGetAngle()) * Math.Sin(x.mGetAngle());
double y_prime_s = yval * Math.Cos(y.mGetAngle()) + zval * Math.Sin(x.mGetAngle());
float cur_s = (float)(Math.Atan2(y_prime_s, x_prime_s)) - (float)(110.0f * Math.PI / 180.0f);
if (cur_s < -Math.PI)
cur_s += (float)(2 * Math.PI);
if (yval < -25.0f)
yval = -25.0f;
else if (yval > 25.0f)
yval = 25.0f;
if (xval < -25.0f)
xval = -25.0f;
else if (xval > 25.0f)
xval = 25.0f;
if (zval < -25.0f)
zval = -25.0f;
else if (zval > 25.0f)
zval = 25.0f;
double x_prime = xval * Math.Cos(x.mGetAngle()) + yval * Math.Sin(y.mGetAngle()) * Math.Sin(x.mGetAngle()) + zval * Math.Cos(y.mGetAngle()) * Math.Sin(x.mGetAngle());
double y_prime = yval * Math.Cos(y.mGetAngle()) - zval * Math.Sin(x.mGetAngle());
float cur = (float)(Math.Atan2(y_prime, x_prime)) - (float)(110.0f * Math.PI / 180.0f);
if (cur < -Math.PI)
cur += (float)(2 * Math.PI);
//mTiltHeadingSum += cur;
//mTiltHeadingSum -= mTiltHeading[mTiltHeadingIndex];
mTiltHeading[mTiltHeadingIndex] = cur;
mTiltHeadingIndex++;
mTiltHeadingIndex = (mTiltHeadingIndex >= mHeadSize) ? 0 : mTiltHeadingIndex;
//mTiltHeadingSumSecond += cur_s;
//mTiltHeadingSumSecond -= mTiltHeadingSecond[mTiltHeadingIndexSecond];
mTiltHeadingSecond[mTiltHeadingIndexSecond] = cur_s;
mTiltHeadingIndexSecond++;
mTiltHeadingIndexSecond = (mTiltHeadingIndexSecond >= mHeadSize) ? 0 : mTiltHeadingIndexSecond;
//if(x.mAccStop && y.mAccStop && z.mAccStop)
//{
// mTiltHeadingAvg = mTiltHeadingSum / mHeadSize;
// mTiltHeadingAvgSecond = mTiltHeadingSumSecond / mHeadSize;
// mStepCountTiltHeadingAvg = mTiltHeadingAvg;
//}
}
private void mUpdateHeading(ref Axis x, ref Axis y)
{
float yval = y.mGetMag();
//if (yval < -25.0f)
//yval = -25.0f;
//else if (yval > 25.0f)
// yval = 25.0f;
float xval = x.mGetMag();
//if (xval < -25.0f)
// xval = -25.0f;
//else if (xval > 25.0f)
// xval = 25.0f;
float cur = (float)Math.Atan2(yval, xval) - (float)(110.0f * Math.PI / 180.0f);
if (cur < -Math.PI)
cur += (float)(2 * Math.PI);
//mHeadingSum += cur;
//mHeadingSum -= mHeading[mHeadingIndex];
mHeading[mHeadingIndex] = cur;
mHeadingIndex++;
mHeadingIndex = (mHeadingIndex >= mHeadSize) ? 0 : mHeadingIndex;
//mHeadingAvg = mHeadingSum / mHeadSize;
}