Simple functions to calculate medians from doubles.

median.cpp

#include "median.h"

#include <string.h>

// Storage for the input to the median filter.
double data[MEDIAN_FILTER_SIZE];
double median_data[MEDIAN_FILTER_SIZE];
// Pointer to the next element in the median filter. Not very safe programming!
double* pNextDataInput = data;
// This will be used when the filter is filling up, but then will be always MEDIAN_FILTER_SIZE
int medianFilterCount = 0;

void insertValue(double value)
{
    // Implements a circular buffer.
    *pNextDataInput = value;
    pNextDataInput++;
    if ((pNextDataInput - data) >= MEDIAN_FILTER_SIZE)
    {
        pNextDataInput = data;
    }

    if (medianFilterCount < MEDIAN_FILTER_SIZE)
    {
        medianFilterCount++;
    }
}

void reset()
{
    // Resets things to their start conditions. Useful if you stop reading the values for some
    // reason.
    medianFilterCount = 0;
    pNextDataInput = data;
}

double getMedian()
{
    // Copy the current data to the median_data structure
    // This looks funny, because data is a circular buffer, but it's either filled from the
    // front, or full, so starting at the 0th element is always OK.
    memcpy(median_data, data, sizeof(double) * medianFilterCount);

    // Sort the first half of the copy
    int medianIndex = medianFilterCount/2; // floor
    for (int low = 0; low < medianIndex + 1; low++)
    {
        for (int high = low+1; high < medianFilterCount; high++)
        {
            if (median_data[low] > median_data[high])
            {
                // swap
                double temp = median_data[low];
                median_data[low] = median_data[high];
                median_data[high] = temp;
            }
        }
    }

    // Return the value from halfway through the sorted data
    return median_data[medianIndex];
}

median.h

#pragma once

// The max number of values stored in the median filter.
#define MEDIAN_FILTER_SIZE 5

// Call with new values to insert into the filter.
void insertValue(double value);

// Call to reset the filter.
void reset();

// Call to retrieve the median from the stored values.
double getMedian();