File content as of revision 0:726c69fad7d8:

#include "Yamartino.h"
#define PI                    3.14159265
// Converts degrees to radians.
#define degreesToRadians(angleDegrees) (angleDegrees * PI / 180.0)
 
// Converts radians to degrees.
#define radiansToDegrees(angleRadians) (angleRadians * 180.0 / PI)

Yamartino::Yamartino() {
    historyLength = 10;
    //history = new float[historyLength]; // keep our history values
    historyX = new float[historyLength];
    historyY = new float[historyLength];
}

float Yamartino::myAtan2(float y, float x) {
  float t = atan2(y, x);
  return t > 0 ? t : 2 * PI + t;
}

float Yamartino::calculateYamartinoAverage() {

  float sumX = 0;
  float sumY = 0;
  
  for (int i = 0; i < historyLength; i++) {
    sumX += historyX[i];
    sumY += historyY[i];
  }

  //float meanX = sumX / history.length;
  //float meanY = sumY / history.length;
  // YAMARTINO METHOD FOR STANDARD DEVIATION!!
  // http://en.wikipedia.org/wiki/Yamartino_method
  //float eps = sqrt(1 - (meanX*meanX + meanY*meanY));
  //eps = Float.isNaN(eps) ? 0 : eps; // correct for NANs
  //historyCorrectStd = asin(eps)* (1 + (2 / sqrt(3) - 1) * (eps * eps * eps));
  
  return radiansToDegrees(myAtan2(sumY, sumX));
}

void Yamartino::addItemsToHistoryBuffers(float input) {
  float valueRadians = degreesToRadians(input);
  //addToHistory(history,input);
  addToHistory(historyX,cos(valueRadians));
  addToHistory(historyY,sin(valueRadians));
}

void Yamartino::addToHistory(float* buffer, float input) {
  // delete the oldest value from the history
  // add one value to the history (the input)
  // take the average of the history and return it;

  // shift the values to the left in the array
  for (int i = historyLength - 1; i >= 0; i--) {
    if (i == 0) {
      buffer[0] = input;
    }
    else {
      buffer[i] = buffer[i-1];
    }
  }
}