File content as of revision 155:7c6005933d4c:

#include "mbed.h"
#include "math.h"
#include "Filter.h"

CircularBuffer::CircularBuffer(int length, bool use_median) {
    _length = length;
    _use_median = use_median;
    
    oldest_index = 0;
    newest_index = -1;
    num = 0;
    sum = 0.0f;
    
    buf = (float*)malloc(_length * sizeof(float));
    sorted = (float*)malloc(_length * sizeof(float));
    for (int i = 0; i < _length; i++) {
        buf[i] = 0.0f;
        sorted[i] = 0.0f;
    }
}

float &CircularBuffer::at(int index) {
    int actual = oldest_index + index;
    if (actual >= _length) actual -= _length;
    return buf[actual];
}

void CircularBuffer::add(float x) {
    if (num < _length) {
        newest_index++;
        buf[newest_index] = x;
        sum += x;
        num++;

        if (!_use_median || _length < 4) return;

        /*insert x into sorted array*/
        int i = num - 1;
        while (i > 0 && sorted[i - 1] > x) {
            sorted[i] = sorted[i - 1];
            i--;
        }
        sorted[i] = x;
    } else {
        /*update circular buffer*/
        float oldest = buf[oldest_index];

        sum -= buf[oldest_index];
        oldest_index++;
        if (oldest_index >= _length) oldest_index -= _length;

        newest_index++;
        if (newest_index >= _length) newest_index -= _length;
        buf[newest_index] = x;

        sum += x;

        if (!_use_median || _length < 4) return;

        /*find sorted index of oldest element*/
        int removed;
        for (removed = 0; removed < _length; removed++) {
            if (sorted[removed] == oldest) break;
        }

        /*insert x*/
        int i;
        if (removed == _length - 1) {
            i = _length - 1;
            while (i > 0 && sorted[i - 1] > x) {
                sorted[i] = sorted[i - 1];
                i--;
            }
            sorted[i] = x;
        } else if (removed == 0) {
            i = 0;
            while (i < _length - 1 && sorted[i + 1] < x) {
                sorted[i] = sorted[i + 1];
                i++;
            }
            sorted[i] = x;
        } else if (sorted[removed - 1] <= x && sorted[removed + 1] >= x) {
            sorted[removed] = x;
        } else if (sorted[removed - 1] > x) {
            i = removed;
            while (i > 0 && sorted[i - 1] > x) {
                sorted[i] = sorted[i - 1];
                i--;
            }
            sorted[i] = x;
        } else {
            i = removed;
            while (i < _length - 1 && sorted[i + 1] < x) {
                sorted[i] = sorted[i + 1];
                i++;
            }
            sorted[i] = x;
        }
    }
}

float CircularBuffer::mean() {
    return sum / num;
}

float median3(float *buf) {
    if (buf[0] > buf[1]) {
        if (buf[1] > buf[2]) {
            return buf[1];
        } else if (buf[0] > buf[2]) {
            return buf[2];
        } else {
            return buf[0];
        }
    } else {
        if (buf[0] > buf[2]) {
            return buf[0];
        } else if (buf[1] > buf[2]) {
            return buf[2];
        } else {
            return buf[1];
        }
    }
}
float CircularBuffer::median() {
    if (_length == 1) {
        return buf[0];
    }
    if (_length == 2) {
        if (num < 2) return buf[0];
        return (buf[0] + buf[1]) / 2.0f;
    }
    if (_length == 3) {
        if (num < 2) return buf[0];
        if (num == 2) return (buf[0] + buf[1]) / 2.0f;
        return median3(buf);
    }
    if (num < _length) {
        if (num % 2 == 1) {
            return sorted[(num - 1) / 2];
        } else {
            return (sorted[num / 2] + sorted[num / 2 - 1]) / 2.0f;
        }
    }
    else {
        if (_length % 2 == 1) {
            return sorted[(_length - 1) / 2];
        } else {
            return (sorted[_length / 2] + sorted[_length / 2 - 1]) / 2.0f;
        }
    }
}