34 lines
1021 B
C
34 lines
1021 B
C
#ifndef _HELPFUNCTIONS_H_
|
|
#define _HELPFUNCTIONS_H_
|
|
|
|
#include "definitions.h"
|
|
|
|
int sort_desc(const void *cmp1, const void *cmp2);
|
|
float filterMedian(int16_t* values);
|
|
|
|
int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
|
|
{
|
|
float a = *((float *)cmp1);
|
|
float b = *((float *)cmp2);
|
|
return a > b ? -1 : (a < b ? 1 : 0);
|
|
}
|
|
|
|
|
|
float filterMedian(int16_t* values) {
|
|
float copied_values[CURRENT_FILTER_SIZE];
|
|
for(int i=0;i<CURRENT_FILTER_SIZE;i++) {
|
|
copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
|
|
}
|
|
float copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
|
|
qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);
|
|
|
|
float mean=copied_values[CURRENT_FILTER_SIZE/2];
|
|
for (uint8_t i=1; i<=CURRENT_MEANVALUECOUNT;i++) {
|
|
mean+=copied_values[CURRENT_FILTER_SIZE/2-i]+copied_values[CURRENT_FILTER_SIZE/2+i]; //add two values around center
|
|
}
|
|
mean/=(1+CURRENT_MEANVALUECOUNT*2);
|
|
|
|
return mean;
|
|
}
|
|
|
|
#endif |