71 lines
1.5 KiB
C
71 lines
1.5 KiB
C
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#define N 8
|
|
|
|
uint16_t buffer[N];
|
|
uint8_t idx = 0;
|
|
|
|
/*
|
|
* Pros: Simple and smooths well.
|
|
* Cons: Lag, slow to respond to real changes.
|
|
*/
|
|
uint16_t moving_average(uint16_t new_sample) {
|
|
buffer[idx++] = new_sample;
|
|
if (idx >= N)
|
|
idx = 0;
|
|
|
|
uint32_t sum = 0;
|
|
for (int i = 0; i < N; i++) sum += buffer[i];
|
|
return sum / N;
|
|
}
|
|
|
|
float alpha = 0.1f; // Between 0 (more smoothing) and 1 (less)
|
|
float filtered = 0;
|
|
|
|
/*
|
|
* Pros: Very lightweight, tunable.
|
|
* Cons: Might not remove all burst noise
|
|
*/
|
|
uint16_t low_pass_filter(uint16_t new_sample) {
|
|
filtered = alpha * new_sample + (1 - alpha) * filtered;
|
|
return (uint16_t)filtered;
|
|
}
|
|
|
|
int compare_uint16(const void *a, const void *b) {
|
|
return (*(uint16_t *)a - *(uint16_t *)b);
|
|
}
|
|
|
|
uint16_t median_filter(uint16_t new_sample) {
|
|
static uint16_t window[N];
|
|
static uint8_t index = 0;
|
|
window[index++] = new_sample;
|
|
if (index >= N)
|
|
index = 0;
|
|
|
|
// Copy and sort
|
|
uint16_t sorted[N];
|
|
memcpy(sorted, window, sizeof(sorted));
|
|
qsort(sorted, N, sizeof(uint16_t), compare_uint16);
|
|
return sorted[N / 2];
|
|
}
|
|
|
|
#undef N
|
|
#define N 5
|
|
float coeffs[N] = {0.1, 0.15, 0.5, 0.15, 0.1}; // Symmetric LPF
|
|
float buffer[N] = {0};
|
|
int index = 0;
|
|
|
|
float fir_filter(float sample) {
|
|
buffer[index] = sample;
|
|
float result = 0;
|
|
int i, j = index;
|
|
for (i = 0; i < N; i++) {
|
|
result += coeffs[i] * buffer[j];
|
|
j = (j - 1 + N) % N;
|
|
}
|
|
index = (index + 1) % N;
|
|
return result;
|
|
}
|
|
|