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