bobbycar/controller_teensy/include/helpfunctions.h

#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