// Determine KMA scale
float DetermineKMA(int index) {
int i, arrIdx, mX, mY, mZ;
float result;
arrIdx = index;
mX = mAxisBuf.tmp_data_x_lcd[arrIdx];
mY = mAxisBuf.tmp_data_y_lcd[arrIdx];
mZ = mAxisBuf.tmp_data_z_lcd[arrIdx];
// get spot value of ACC(n)
ACCn = (int)sqrt( (mX) * (mX) + (mY) * (mY) + (mZ) * (mZ) );
if(KMACount < ACCnFifty - 1) {
ACCnArr[KMACount] = ACCn;
KMACount++;
} else if(KMACount == ACCnFifty - 1) {
ACCnArr[KMACount] = ACCn;
for(i=0; i<KMACount; i++) {
ACCnAddition += ACCnArr[i];
}
MeanACC = ACCnAddition / 50;
result = ExtractMaxMeanACC(MeanACC);
ACCnAddition = 0; // Clear addition
KMACount++;
} else { // KMACount >= 50
ShiftArrayAndAddData(ACCnArr, ACCn, sizeof(ACCnArr) / sizeof(*ACCnArr));
for(i=0; i<KMACount; i++) {
ACCnAddition += ACCnArr[i];
}
MeanACC = ACCnAddition / 50;
result = ExtractMaxMeanACC(MeanACC);
ACCnAddition = 0; // Clear addition
if(KMACount >= 0x400) { // 1024
KMACount = ACCnFifty;
} else {
KMACount++;
}
}
return result;
/*
int GalDataPlusOne = mAxisData.data_g[index+1];
int GalDataIndex = mAxisData.data_g[index];
if(GalDataPlusOne > GalDataIndex) {
max_gal = mAxisData.data_g[index+1];
} else {
max_gal = max_gal;
}
*/
}
// Determine KMA scale
int DetermineKMA(int index) {
int i, arrIdx, mX, mY, mZ;
int result;
arrIdx = index;
CheckSignAndToInt(arrIdx);
mX = mAxisBuf.tmp_data_x_lcd[arrIdx] >> 2;
mY = mAxisBuf.tmp_data_y_lcd[arrIdx] >> 2;
mZ = mAxisBuf.tmp_data_z_lcd[arrIdx] >> 2;
// get spot value of ACC(n)
ACCn = (int)sqrt( ((mX) * (mX)) + ((mY) * (mY)) + ((mZ) * (mZ)) );
if(KMACount < ACCnFifty - 1) {
ACCnArr[KMACount] = ACCn;
} else if(KMACount == ACCnFifty - 1) {
ACCnArr[KMACount] = ACCn;
for(i=0; i<ACCnFifty; i++) {
ACCnAddition += ACCnArr[i];
}
MeanACC = ACCnAddition / 50;
result = ExtractMaxMeanACC(MeanACC);
ACCnAddition = 0; // Clear addition
//return result;
} else { // KMACount >= 50
ShiftArrayAndAddData(ACCnArr, ACCn, sizeof(ACCnArr) / sizeof(*ACCnArr));
for(i=0; i<ACCnFifty; i++) {
ACCnAddition += ACCnArr[i];
}
MeanACC = ACCnAddition / 50;
result = ExtractMaxMeanACC(MeanACC);
ACCnAddition = 0; // Clear addition
}
if(KMACount != 50) {
KMACount++;
}
return result;
}
void ATFCAlgorithm(int mData) {
int data = mData;
//ATFC:
/* Calculation of ATFC */
if (SampleCount == 0) {
Qt = abs(data);
SumQf = abs(0 - abs(data));
pData = data; // fill in previous data for the first time
Smp_Buf[SampleCount] = mData; // fill in first data to the newest address of the array
} else if (SampleCount < L - 1) { // SampleCount < 49
Qt += abs(data);
SumQf += abs(abs(pData) - abs(data)); // subtract current data from previous data
pData = data; // fill in previous data
Smp_Buf[SampleCount] = data; // fill in incoming data to the newest address of the array
} else if (SampleCount == L - 1) { // SampleCount = 49
Qt += abs(data);
SumQf += abs(abs(pData) - abs(data));
Smp_Buf[SampleCount] = data;// fill in last 50th data to the newest address of the array
Qf = Alpha * SumQf;
ATFCn = Qt + Qf;
/*
printf("\rSampleCount = %d, ", SampleCount);
printf("\tQt = %d, ", Qt);
printf("\tSumQf = %d, ", SumQf);
printf("\tQf = %d, ", Qf);
printf("\tATFCn = %d", ATFCn);
*/
} else { // SampleCount >= 50
Qt = 0; // reset Qt
if (SampleCount == 50) {
OldestSumQfValue = abs(0 - abs(Smp_Buf[0]));
NewestSumQfValue = abs(abs(Smp_Buf[49]) - abs(data));
} else {
OldestSumQfValue = abs(abs(OldestArrayValue) - abs(Smp_Buf[0]));
}
OldestArrayValue = ShiftArrayAndAddData(Smp_Buf, data,
sizeof(Smp_Buf) / sizeof(*Smp_Buf));
int arrIdx;
for (arrIdx = 0; arrIdx < L; arrIdx++) {
Qt += abs(Smp_Buf[arrIdx]);
}
NewestSumQfValue = abs(abs(Smp_Buf[48]) - abs(Smp_Buf[49]));
SumQf = SumQf - OldestSumQfValue + NewestSumQfValue;
Qf = Alpha * SumQf;
ATFCn = Qt + Qf;
if (SampleCount < 52) {
printf("\rSampleCount = %d, ", SampleCount);
printf("\tQt = %d, ", Qt);
printf("\tSumQf = %d, ", SumQf);
printf("\tQf = %d, ", Qf);
printf("\tATFCn = %d", ATFCn);
}
if (SampleCount > 4238 && SampleCount < 4244) {
printf("\rSampleCount = %d,", SampleCount);
printf("\tQt = %d,", Qt);
printf("\tSumQf = %d,", SumQf);
printf("\tQf = %d,", Qf);
printf("\tATFCn = %d", ATFCn);
}
if (isThisFirstBoot) {
goto PARAMINIT;
} else {
goto CHECKREFRESHTIME;
}
/* Parameter Initialization */
PARAMINIT: if (SampleCount < T + 50) { // SampleCount < 1050
mSumATFCn += ATFCn;
switch (SampleCount) {
case 99:
case 199:
case 299:
case 399:
case 499:
case 599:
case 699:
case 799:
case 899:
IstSumATFC[IstSumCount] = mSumATFCn;
mSumATFCn = 0; // reset mSumATFCn to restart add sequence
IstSumCount++;
break;
case 999:
IstSumATFC[IstSumCount] = mSumATFCn;
mSumATFCn = 0; // reset mSumATFCn to restart add sequence
int i;
for (i = 0; i < 10; i++) {
SumATFCn += IstSumATFC[i];
}
printf("\rSampleCount = %d,", SampleCount);
printf("\tIstSumCount=%d,", IstSumCount);
printf("\tSumATFCn = %d,", SumATFCn);
printf("\tmSumATFCn = %d,", mSumATFCn);
printf("\tIstSumATFC[%d] = %d", IstSumCount, IstSumATFC[IstSumCount]);
break;
}
/*
if (SampleCount == T - 1) { // SampleCount = 999
printf("\rSampleCount = %d,", SampleCount);
printf("\tQt = %d,", Qt);
printf("\tSumQf = %d,", SumQf);
printf("\tQf = %d,", Qf);
printf("\tATFCn = %d", ATFCn);
printf("\tSumATFCn = %d,", SumATFCn);
printf("\tTHref = %d,", THref);
printf("\tTHn = %d", THn);
}
*/
if (SampleCount == T + 49) { // SampleCount = 1049
THref = SumATFCn >> 9; // instead of multiply of (2/T), divide 512 by shift 9 times
THn = THref;
isThisFirstBoot = false;
goto CHECKREFRESHTIME;
printf("\rSampleCount = %d,", SampleCount);
printf("\tQt = %d,", Qt);
printf("\tSumQf = %d,", SumQf);
printf("\tQf = %d,", Qf);
printf("\tATFCn = %d", ATFCn);
printf("\tSumATFCn = %d,", SumATFCn);
printf("\tTHref = %d,", THref);
printf("\tTHn = %d", THn);
}
goto INCREASECOUNT;
}
