Filter.cpp

Go to the documentation of this file.

 
#include "Filter.h"
#include "EnigmaIOTdebug.h"
 
float FilterClass::addValue (float value) {
    switch (_filterType) {
    case AVERAGE_FILTER:
        return aveFilter (value);
        break;
    case MEDIAN_FILTER:
        return medianFilter (value);
        break;
    default:
        return value;
    }
}
 
float FilterClass::addWeigth (float coeff) {
    float sumWeight = 0;
 
    for (int i = _order - 1; i > 0; i--) {
        _weightValues[i] = _weightValues[i - 1];
    }
    _weightValues[0] = coeff;
 
    for (int i = 0; i < _order; i++) {
        sumWeight += _weightValues[i];
    }
 
    //DEBUG_VERBOSE ("SumWeight: %f", sumWeight);
 
    return sumWeight;
}
 
 
float FilterClass::aveFilter (float value) {
    float sumValue = 0;
    float sumWeight = 0;
    float procValue;
    int left, right;
 
    for (int i = 0; i < _order - 1; i++) {
        _rawValues[i] = _rawValues[i + 1];
    }
    _rawValues[_order - 1] = value;
 
    DEBUG_VERBOSE ("Value: %f\n", value);
 
    DEBUG_VERBOSE ("Raw values:");
    for (int i = 0; i < _order; i++) {
        DEBUG_VERBOSE (" %f", _rawValues[i]);
    }
 
    DEBUG_VERBOSE ("Coeffs:");
    for (int i = 0; i < _order; i++) {
        DEBUG_VERBOSE (" %f", _weightValues[i]);
    }
 
    if (_index < _order) {
        _index++;
        left = _order - _index;
        right = _order - 1;
    }   else {
        left = 0;
        right = _order - 1;
    }
    DEBUG_VERBOSE ("Index: %d , left: %d , right: %d\n", _index, left, right);
 
    for (int i = left; i <= right; i++) {
        sumValue += _rawValues[i] * _weightValues[i];
        sumWeight += _weightValues[i];
        //DBG_OUTPUT_PORT.printf("Raw value %d: %f\n", (i + 1), _rawValues[_order - (i+1)]);
    }
    DEBUG_VERBOSE ("Sum: %f", sumValue);
    DEBUG_VERBOSE (" SumWeight: %f\n", sumWeight);
 
    procValue = sumValue / sumWeight;
 
    DEBUG_VERBOSE ("Average: %f\n", procValue);
 
    return procValue;
}
 
int FilterClass::divide (float* array, int start, int end) {
    int left;
    int right;
    float pivot;
    float temp;
 
    pivot = array[start];
    left = start;
    right = end;
 
    // While indexes do not cross
    while (left < right) {
        while (array[right] > pivot) {
            right--;
        }
 
        while ((left < right) && (array[left] <= pivot)) {
            left++;
        }
 
        // If indexes have not crossed yet we continue doing exchanges
        if (left < right) {
            temp = array[left];
            array[left] = array[right];
            array[right] = temp;
        }
    }
 
    // Indexes have crossed. We put the pivot on place
    temp = array[right];
    array[right] = array[start];
    array[start] = temp;
 
    // NEw pivot position
    return right;
}
 
void FilterClass::clear () {
    for (int i = 0; i < _order; i++) {
        _rawValues[i] = 0;
        _orderedValues[i] = 0;
        //_weightValues[i] = 1;
    }
    _index = 0;
}
 
FilterClass::~FilterClass () {
    free (_rawValues);
    free (_orderedValues);
    free (_weightValues);
}
 
void FilterClass::quicksort (float* array, int start, int end) {
    float pivot;
 
    if (start < end) {
        pivot = divide (array, start, end);
 
        // Ordeno la lista de los menores
        quicksort (array, start, pivot - 1);
 
        // Ordeno la lista de los mayores
        quicksort (array, pivot + 1, end);
    }
}
 
float FilterClass::medianFilter (float value) {
    float procValue;
    int medianIdx;
    int left, right, tempidx;
    bool even;
 
    if (_index < _order) {
        _index++;
        left = _order - _index;
        right = _order - 1;
        even = ((right - left) % 2) == 1;
        DEBUG_VERBOSE ("%d: ", (right - left) % 2);
        if (even) {
            tempidx = (right - left - 1) / 2;
            DEBUG_VERBOSE ("even\n");
        }       else {
            tempidx = (right - left) / 2;
            DEBUG_VERBOSE ("odd\n");
        }
        medianIdx = right - _index + 1 + tempidx;
    }   else {
        left = 0;
        right = _order - 1;
        even = (_order % 2) == 0;
        if (even)
            tempidx = (right - 1) / 2;
        else
            tempidx = right / 2;
        medianIdx = right - _index + 1 + tempidx;
    }
    DEBUG_VERBOSE ("Index: %d , left: %d , right: %d , even: %s , tempidx: %d , medianidx: %d\n", _index, left, right, (even ? "even" : "odd"), tempidx, medianIdx);
 
    // Shift raw values
    for (int i = 0; i < _order - 1; i++) {
        _rawValues[i] = _rawValues[i + 1];
    }
    // Add new raw value
    _rawValues[_order - 1] = value;
 
    DEBUG_VERBOSE ("Raw values:");
    for (int i = 0; i < _order; i++) {
        DEBUG_VERBOSE (" %f", _rawValues[i]);
    }
 
    // copy to array before ordering
    for (int i = 0; i < _order; i++) {
        _orderedValues[i] = _rawValues[i];
    }
 
    // order values
    quicksort (_orderedValues, left, right);
 
    DEBUG_VERBOSE ("Ordered values:");
    for (int i = 0; i < _order; i++) {
        DEBUG_VERBOSE (" %f", _orderedValues[i]);
    }
 
    // select median value
    if (!even) {
        procValue = _orderedValues[medianIdx];
    }   else { // there is no center value
        procValue = (_orderedValues[medianIdx] + _orderedValues[medianIdx + 1]) / 2.0F;
    }
 
    DEBUG_VERBOSE ("Median: %f\n", procValue);
    return procValue; // return mid value
}
 
FilterClass::FilterClass (FilterType_t type, uint8_t order) {
    _filterType = type;
 
    if (order < MAX_ORDER)
        if (order > 1)
            _order = order;
        else
            _order = MIN_ORDER;
    else
        _order = MAX_ORDER;
 
    _rawValues = (float*)malloc (_order * sizeof (float));
    for (int i = 0; i < _order; i++) {
        _rawValues[i] = 0;
    }
 
    _orderedValues = (float*)malloc (_order * sizeof (float));
    for (int i = 0; i < _order; i++) {
        _orderedValues[i] = 0;
    }
 
    _weightValues = (float*)malloc (_order * sizeof (float));
    for (int i = 0; i < _order; i++) {
        _weightValues[i] = 1;
    }
 
}