Arduino 3DOF Head Tracker

Here is the new code with digital high-pass filtering for drift reduction.

#include <math.h>
#include <string.h>

#define gPin 0    
#define xAccePin 1
#define yAccePin 2
#define zAccePin 3
#define ledPin 7  
#define rledPin 7  
#define Vin 322
#define xOffset 508
#define yOffset 508
#define zOffset 552


float accelAlpha, gAlpha;

int gOffset = 0;
long gRaw = 0, gRawOld = 0, xRaw = 0, yRaw = 0, zRaw = 0;
unsigned long timeold_fast = 0, timeold_med = 0;       
float rate = 0, rateold = 0, angle = 0, Azi = 0, Ele = 0, Roll = 0, AziOld = 0, EleOld = 0, RollOld = 0;
char tempc[10], printStr[50];

void GyroZero(unsigned int n) {
  long tempG = 0;

  for(unsigned int k = 1; k <= n; k++){
    tempG += analogRead(gPin);  
    delay(1);
  }
  gRaw = tempG/n;
}

void A2Ddata() {
  gRaw = analogRead(gPin);
  xRaw = analogRead(xAccePin)-xOffset;
  yRaw = analogRead(yAccePin)-yOffset;
  zRaw = analogRead(zAccePin)-zOffset;
}

void setup() {
  pinMode(ledPin, OUTPUT);  // declare the ledPin as an OUTPUT
  pinMode(rledPin, OUTPUT);
  Serial.begin(38400);        // use the serial port to send the values back to the computer

  analogReference(EXTERNAL);
  //Serial.print("Starting...");


  accelAlpha = 0.01/(1/(2*3.1416*2.5+0.01));    //compute accelerometer low-pass filter value, cut off freq = 2.5 hz
  gAlpha = (1/(2*3.1416*0.01)) / (1/(2*3.1416*0.01)+0.01); //compute gyro high-pass filter value, cut off freq = 0.01 hz


  digitalWrite(rledPin,HIGH);
  delay(2000);
  GyroZero(1000);
  gOffset = gRaw*Vin/100;
  gRawOld = 0;

  printStr[0]= '\0';
  digitalWrite(rledPin,LOW);
}



void loop() {

  if (millis()-timeold_fast > 10) {
    timeold_fast = millis();
    A2Ddata();

    //Calculate gyro turn rate using digital high-pass filter
    gRaw = gRaw*Vin/100-gOffset;
    rate = gAlpha*(rateold + (gRaw - gRawOld)*0.150);
    
    if (abs(rate) > 4) {
      angle += (rateold+rate)*0.005;  //trapz intergration
    }
    rateold = rate;
    gRawOld = gRaw;

    //Calculate elevation and roll angles using digital low-pass filter
    Ele = accelAlpha*(atan2(zRaw,xRaw)*57.296-90-EleOld) + EleOld;
    Roll = accelAlpha*(atan2(zRaw,yRaw)*57.296-90-RollOld) + RollOld;

    if (abs(Ele - EleOld) > 0.5) {
      EleOld = Ele;
    }
    else {
      Ele = EleOld;
    }

    if (abs(Roll - RollOld) > 0.5) {
      RollOld = Roll;
    }
    else {
      Roll = RollOld;
    }

    EleOld = Ele;
    RollOld = Roll;
  }

  if (millis()-timeold_med > 25) {
    timeold_med = millis();

    if (Ele < -30) {
      angle = 0;
    }

    digitalWrite(ledPin, HIGH); 

    printStr[0] = 'K';
    printStr[1] = '\0';
    strcat(printStr," ,");
    strcat(printStr,floatToString(tempc,Ele,1,0));
    strcat(printStr,", ");
    strcat(printStr,",");
    strcat(printStr,floatToString(tempc,-angle,1,0));
    Serial.println(printStr);
    printStr[0] = '\0';

    digitalWrite(ledPin, LOW);  
  }

}