Hi,
I've a circular buffer presently holding up to 30 reads (period of a frequency). Occasionally some noise creeps in which I need to deal with prior to calculating the mean and co-efficient of variation, which in turn is used to mark whether the frequency is 'stable'. It would be great to have a few eyes go over what I came up with to see if there's any errors. The buffer fills at 10Hz.
struct motionReadProto { uint8_t outlier; uint16_t value; }; struct motionProto { uint16_t mean; uint8_t stable; uint8_t ready; }; static struct motionReadProto buffer[MOTION_BUFFER_COUNT]; static struct motionReadProto *currentBuffer = &buffer[0]; volatile struct motionProto motion; static uint32_t squareRootRounded(uint16_t op) { uint32_t res = 0; uint16_t one = 1u << 14; // The second-to-top bit is set: use 1u << 14 for uint16_t type; use 1uL<<30 for uint32_t type // "one" starts at the highest power of four <= than the argument. while (one > op) { one >>= 2; } while (one != 0) { if (op >= res + one) { op = op - (res + one); res = res + 2 * one; } res >>= 1; one >>= 2; } /* Do arithmetic rounding to nearest integer */ if (op > res) { res++; } return res; } void MOTION_Task (void) { if (! TIMER_INT_FLAG) { return; } // move buffer if (currentBuffer == &buffer[MOTION_BUFFER_COUNT-1]) { currentBuffer = &buffer[0]; motion.ready = 1; } else { currentBuffer++; } // save result currentBuffer->value = TIMER_CAPTURE_VALUE; // timer will clear int flag automatically uint8_t i; uint8_t reads = 0; // mean uint32_t mean = 0; for (i=0; i < MOTION_BUFFER_COUNT; ++i) { if (buffer[i].value) { mean += buffer[i].value; ++reads; } } mean /= reads; // deltas uint32_t delta = 0; for (i=0; i < MOTION_BUFFER_COUNT; ++i) { if (buffer[i].value) { uint16_t x = (buffer[i].value - (uint16_t) mean); delta += x*x; } } delta /= reads; // standard deviation uint16_t sd = squareRootRounded(delta); // mark outliers for (i=0; i < MOTION_BUFFER_COUNT; ++i) { buffer[i].outlier = !buffer[i].value || (buffer[i].value < mean - 2*sd) || (buffer[i].value > mean + 2*sd)? 1:0; } // recompute mean and sd without outliers mean = 0; reads = 0; for (i=0; i < MOTION_BUFFER_COUNT; ++i) { if (! buffer[i].outlier) { mean += buffer[i].value; ++reads; } } motion.mean = mean / reads; // deltas delta = 0; for (i=0; i < MOTION_BUFFER_COUNT; ++i) { if (! buffer[i].outlier) { uint16_t x = (buffer[i].value - motion.mean); delta += x*x; } } delta /= reads; // standard deviation sd = squareRootRounded(delta); // co-efficient of variation uint8_t cov = motion.mean? ((sd*100) / motion.mean) : 0; //outcome motion.stable = motion.ready && cov < MOTION_STABILITY_MAX_COV? 1:0; }