Arduino 3DOF Head Tracker

I posted it at RC Groups, but a couple of people have asked me for the code, which I thought would be more appropriate to post here.
(For more detail, check this thread:
DIY head tracker from cheap heli gyro (seems to work!) - RC Groups)

But I thought people here might be interested in it, especially the code.

It's a 3 degree of freedom (pitch, roll, yaw) head tracker built from:

  • A cheap RC heli gyro for ~$15
  • A cheap 3 axis accelerometer board for ~$16
  • RBBB for ~$12

The final goal is to fly my FPV plane through VR goggles, but I'm currently testing it with Microsoft FSX, here's a video:

Here's the code, will need to be modified to suit you own setup. Also, if anyone wants it, I've got a program that interfaces with FSX camera control and serial port, all you need to do is send a comma separated string of pitch, roll yaw and so on. Should make it easy for anyone wanting to control FSX camera from arduino.

#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 5081
#define yOffset 5080
#define zOffset 5517


int gOffset = 0;//, xOffset = 0, yOffset = 0, zOffset = 0;
long gRaw = 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 A2Ddata(unsigned int n) {
  long tempG = 0, tempX = 0, tempY = 0, tempZ = 0;

  for(unsigned int k = 1; k <= n; k++){
    tempG += analogRead(gPin);  
    tempX += analogRead(xAccePin);
    tempY += analogRead(yAccePin);
    tempZ += analogRead(zAccePin);
    //delayMicroseconds(10);
  }
  gRaw = tempG*10/n;
  xRaw = tempX*10/n-xOffset;
  yRaw = tempY*10/n-yOffset;
  zRaw = tempZ*10/n-zOffset;
}


void setup() {
  pinMode(ledPin, OUTPUT);  
  pinMode(rledPin, OUTPUT);
  Serial.begin(38400);        

  analogReference(EXTERNAL);
  digitalWrite(rledPin,HIGH);
  delay(1000);
  //Find Gyro rate offset
  A2Ddata(10000);
  gOffset = gRaw*Vin/100;
  printStr[0]= '\0';
  digitalWrite(rledPin,LOW);
}



void loop() {

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

    //Calculate gyro turn rate
    rate = (gRaw*Vin/100-gOffset)*0.0150;

    if (abs(rate) > 2.5) {
      angle += (rateold+rate)*0.010;  //trapz intergration
    }
    rateold = rate;

    //Calculate elevation and roll angles
    Ele = (atan2(zRaw,xRaw)*57.296-90+EleOld)/2;
    Roll = (atan2(zRaw,yRaw)*57.296-90+RollOld)/2;

  }

  if (millis()-timeold_med > 50) {
    timeold_med = millis();
    // hysterisis dead band
    if (abs(Ele - EleOld) > 0.5) {
      EleOld = Ele;
    }
    else {
      Ele = EleOld;
    }

    if (abs(Roll - RollOld) > 0.5) {
      RollOld = Roll;
    }
    else {
      Roll = RollOld;
    }
    
// reset yaw angle to 0 when head is lowered > 60 degrees
    if (Ele < -60) {
      angle = 0;
    }
   
/*
// code to reduce gyro drift under steady conditions 
    if (rate < 5) {
      if (rate > 0) {
        gOffset++;
      }
      else{
        gOffset--;
      }
    }
*/
    digitalWrite(ledPin, HIGH); 

    strcat(printStr,floatToString(tempc,rate,2,6,false));
    strcat(printStr,",");
    strcat(printStr,floatToString(tempc,Ele,3,6,false));
    strcat(printStr,",");
    strcat(printStr,floatToString(tempc,Roll,3,6,false));
    strcat(printStr,",");
    strcat(printStr,floatToString(tempc,angle,3,6,false));
    Serial.println(printStr);

    printStr[0] = '\0';
    /*
    Serial.print(floatToString(tempc,rate,2,6,false));
     Serial.print(','); 
     //Serial.println(floatToString(tempc,angle,2,6,false));   
     Serial.print(floatToString(tempc,Ele,2,6,false));
     Serial.print(','); 
     Serial.print(floatToString(tempc,Roll,2,6,false));
     Serial.print(','); 
     Serial.println(floatToString(tempc,angle,2,6,false));
     */
    digitalWrite(ledPin, LOW);  
  }

}

Wow, this is very cool. I recently got into Arduino boards, and electronics in general, because I wanted to make a few physical controllers for FSX. I have since completed a switch panel for controlling things like magnetos, flaps, lights and fuel selection. I'm currently planning out an auto-pilot interface with an LCD screen and some fancy light up buttons. Would you mind posting the code you wrote to interface with FSX? I'd like to compare it with mine to see if I could improve it.

Also, if anyone is interested, I could post some pictures of my build, perhaps in another thread.

Thanks!

Hi,

I didn't really write the code for interfacing with FSX. I took a SimConnect SDK example in Delphi, and modify it to take serial data, that's about it... If that's what you want to look at, I'll post it here...

Cheers,
-Z-

ah, don't worry about it then. I used C# for some reason (Java programmer at work, I figured it was close enough :slight_smile: ) It's fairly simply anyway.

Thanks for the inspiration though, this is neat!

Well Done work..Does your code eliminate bias drift of the yaw gyro, exactly?

No, it does not eliminate gyro drift. I've programmed it to recentre when I lower my head. You can see me do that a couple of times in the video.

I have some questions.if you could answer i would be glad.In your code,

void A2Ddata(unsigned int n) {
long tempG = 0, tempX = 0, tempY = 0, tempZ = 0;

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

???? what the value you get ? is it radians/sec or deg/sec ? is tempG given you as raw value of the sensor ?
especially i didnt understand the temG*10/n ? what this calc. do?

A2Ddata(8); ???? why did you send 8 ?

//Calculate gyro turn rate
rate = (gRaw*Vin/100-gOffset)*0.0150; ??

if (abs(rate) > 2.5) {
angle += (rateold+rate)*0.010; //trapz intergration ???
}
rateold = rate;

?? what this rate means ?
??? this should be the integration to calculate angle.

// code to reduce gyro drift under steady conditions
if (rate < 5) {
if (rate > 0) {
gOffset++;
}
else{
gOffset--;
}
}

Did you use this in your code?

I am sorry about these question, it is a lot. But really i need these because i am at the end of edge my thesis. Thanks your help

void A2Ddata(unsigned int n) {
long tempG = 0, tempX = 0, tempY = 0, tempZ = 0;

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

???? what the value you get ? is it radians/sec or deg/sec ? is tempG given you as raw value of the sensor ?
especially i didnt understand the temG*10/n ? what this calc. do?

Uhm... A2Ddata(n) simply averages n readings from the A2D converter. If you take the average of many readings from a noisy sensor, the average will have more resolution than the A2D converter readings, so by using temG*10/n I keep some of the increased resolution. So A2Ddata(8) takes the average of 8 readings. These are just raw A2D readings.

rate = (gRaw*Vin/100-gOffset)*0.0150;

gRaw is the raw gyro value10, Vin/100 is the A2D full scale voltage (322/100) or 3.22V. This voltage is feed to the VRef pin of arduino. gOffset is the voltage from the gyro when it's stationary10, and 0.0150 finally converts everything into degree/s. I'm assuming my gyro has a response of 150dgr/s/V output, it's pretty close to that.

angle += (rateold+rate)*0.010;  //trapz intergration
rateold = rate;

This does a second order trapezoidal integration, the formula is:

y_n = y_n-1 + ( x_n + x_n-1 )/2 *dt

dt/2 in my case is approximately 0.010

Did you use this in your code?

That bit of code does work to reduce drift when the head tracker is stationary, but it's not good when it's moving.

My code is very basic. I'm working on a better drift removal method by using a digital high-pass filter.

Hope this helps,
-Z-

Thank you, i really appreciate your help.

Check out the Angle complementary filter on this site: The DIY Segway Its in the Segspecs.zip file, in PDF form. Its for filtering out drift using Gyro and Accel, mainly for a segway, but I think it would work for this.

Hey kersny,

Thanks for the link, I read about their filter, very interesting. I have been testing my own digital high pass filter to get rid of some of the drift. The problem is for pure panning motion, there is no accelerometer information available to correct the gyro, so their code will not work. I think it may be possible to correct drift when your head is not perfectly straight up though, but I have yet to get my head around it (so to speak :D).

-Z-

hi,
yikes, I have all these pieces, and would like to put them together! Could I ask you to post a schematic, or more detailed pictures of the build, so I can see your wiring... at a glance, it isn't all obvious to me.

thanks,
david

waxweb,

It's not very difficult to put them together, but which parts do you have? The only difficulty is figuring out what where the gyro outputs the analogue signal, but that is different for different gyros.

-Z-

hi Zitron,
I have a sure electronics accelerometer, and a rbbb, but missed the gyro on the list... which is the one you used, from the page linked to at r2hobbies? Right off, looking at your photo, I was confused by: the trim pot [is that the gyro?]; the connection between + and J2/sleep; and what appears to be an extra resistor off the bottom of the rbbb. I ask 'cause, as a noob, small errors repeat endlessly when guessing a build. thanks, and cool work! david

Hi,

Sorry about the bad photos, I'll try to take some better ones.

The gyro is KEY! The point I was trying to make was that it may be possible to use the parts from one cheap RC gyro for ~$15 instead of a more expensive high performance gyro from Sparkfun, Analog Devices, etc. Currently head trackers for FPV flying use 2 high quality gyros or tilt compensated compasses, which is why they are so expensive ($150-250).

The gyro has the trim pot on top of it, it's used to create a reference voltage for either the gyro itself or the OpAmp, I can't remember.

Both the gyro and the accelerometer operate at ~3V so I'm sending the accelerometer 3.3V supply to the AREF pin on the 168, through a ~500R resistor. The resistor is supposed to help to prevent sinking to much current into the pin in case if you forget to set the pin as external voltage reference (or something like that). The sleep (J2) pin on the accelerometer board needs to be connected to 3.3V for the damn thing to work, otherwise it will stay in sleep mode! I found that one out the hard way!

Also, what is your end goal? Extra stuff are needed if you want to interface it with a computer program, or plug it into a RC transmitter.

Cheers,
-Z-

Got it working with my RC transmitter controlling a pan-tilt camera:

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);  
  }

}

This is a very interesting topic for me since I am trying to figure out how to connect a Gyro from a helicopter to my arduino.

How can you determine where the PWM signal is coming out of the Gyro? Is there a test you can do without having a scope?

Thanks in advance for any info and please step lightly on this NEW B.

Jim

How can you determine where the PWM signal is coming out of the Gyro? Is there a test you can do without having a scope?

Hi,

I've decided not to read the PWM coming out, even though it's more convenient. Instead, I took apart the gyro to find the amplified analogue signal, before it's fed into the uC inside the gyro itself. My reasoning is that the PWM is limited in resolution and refresh rate, analogRead() is much faster, and I do not need to waste time processing the PWM. Also, the gyro uC probably also does some of its own processing, so the output signal is probably not directly proportional to gyro rate, so it won't be suitable for use in a head tracker.

You should be able to find the analogue signal easily by poking around with a multimeter, that's what I did :). If you need the gyro to control the heli by itself, you can probably solder a wire from it and plug the other end into the arduino...

Hope that helps,
-Z-