#define FILTER_SIZE 10
float accelX[FILTER_SIZE];
float accelY[FILTER_SIZE];
float accelZ[FILTER_SIZE];
int filterIndex = 0;
void addAccelData(float x, float y, float z) {
accelX[filterIndex] = x;
accelY[filterIndex] = y;
accelZ[filterIndex] = z;
filterIndex = (filterIndex + 1) % FILTER_SIZE;
}
void getSmoothedAccelData(float *x, float *y, float *z) {
float sumX = 0, sumY = 0, sumZ = 0;
for (int i = 0; i < FILTER_SIZE; i++) {
sumX += accelX[i];
sumY += accelY[i];
sumZ += accelZ[i];
}
*x = sumX / FILTER_SIZE;
*y = sumY / FILTER_SIZE;
*z = sumZ / FILTER_SIZE;
}
|
