File content as of revision 8:8430a5c0914c:

#include "mbed.h"
#include "process_data.h"
#include "SystemTime.h"
#include "debug.h"

int window = 0;
int16_t *history;
int16_t prev_data = 0;

Timer split_timer;

/* Initializes the moving average with a window size */
bool process_init()
{
    window = WINDOW_SIZE;
    history = new int16_t[WINDOW_SIZE](); // initializes to 0

    split_timer.start();
    return true;
}

/* Will break for window size > 2^8
 * Returns a moving average for data
 * Returns INT16_MAX until it has received a window
 * size worth of data. These values should be ignored. */
int16_t mov_avg(int16_t data)
{
    static uint8_t front = 0, end_ = 0, count = 0;
    static int32_t total;

    if (count < window-1) {
        history[end_] = data;
        end_ = end_ + 1;
        count = count + 1;
        return INT16_MAX;
    } else if (count == window-1) {
        history[end_] = data;
        for (int i = 0; i <= count; i++)
            total = total + history[i];
        count = count + 1;
        end_ = 0;
        front = 1;
        prev_data = history[0];
        return INT16_MAX;
    } else {
        history[end_] = data;
        total = total - prev_data + data;
        end_ = end_ + 1;
        front = front + 1;

        if (end_ >= window)
            end_ = 0;

        if (front >= window)
            front = 0;

        if (front == 0)
            prev_data = history[window-1];
        else {
            prev_data = history[front - 1];
        }

        return total/window;
    }
}

/* Returns true if there is a peak (flip turn) */
bool peak_detect(int16_t data)
{
    static uint8_t i = 0;
    static int16_t avg[3];

    int16_t m = mov_avg(data);

    if (m == INT16_MAX)
        return false;

    if (i < 3) {
        avg[i++] = m;
        return false;
    }

    avg[0] = avg[1];
    avg[1] = avg[2];
    avg[2] = m;

    if ( (avg[0] > avg[1]) && (avg[2] > avg[1]) && (avg[2] < -2000) && (avg[1] < -2000) && (avg[0] < -2000) ) {
        PC_PRINTLNF("Peak detected @ %d", split_timer.read_ms());
        return true;
    }

    return false;
}

enum length {ZERO, ONE, TWO};

/* Returns split time, or UINT16_MAX if not on a split */
uint16_t process_data(int16_t data)
{
    static enum length Length = ZERO;
    static int l_zero, l_two;

    if (peak_detect(data)) {
        if (Length == ZERO) {
            l_zero = split_timer.read_ms();
            Length = ONE;
            return UINT16_MAX;
        } else if (Length == ONE) {
//            l_one = SystemTime::read_ms();
            Length = TWO;
            return UINT16_MAX;
        } else {
            l_two = split_timer.read_ms();
            Length = ZERO;
            split_timer.reset();
            return ( abs((l_two - l_zero)));
        }
    }
    
    return UINT16_MAX;
}

bool process_close() {
    delete(history);
    split_timer.reset();
    split_timer.stop();
    return true;
}