Arduino 3DOF Head Tracker

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-

COuldnt you use a dual axis Gyro only and skip the accelerometer?

just wondering about wayts to simplify.

Hi all,

You may be interested to know about the Kalman filter, I have used kalman filters to remove the noise and drift from the gyros and merge the readings with the angle computed from the accelerometers.

There is code available for 6DOF computation in the Arduino Helicopter Autopilot thread.
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1229733416

This may help - may not... depends how much headache you are prepared to put up with! It's certainly not simple, but the results are spectacularly improved.

Hope that helps,

Ed

COuldnt you use a dual axis Gyro only and skip the accelerometer?

Gyro = expensive, complicated and drifts
Accelerometer = cheap, simple does not drift

The point was to only use gyro on the axis that can only be detected by the gyro, for everything else use the accelerometer.

You may be interested to know about the Kalman filter, I have used kalman filters to remove the noise and drift from the gyros and merge the readings with the angle computed from the accelerometers.

There is code available for 6DOF computation in the Arduino Helicopter Autopilot thread.
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1229733416

This may help - may not... depends how much headache you are prepared to put up with! It's certainly not simple, but the results are spectacularly improved.

Hope that helps,

Hey,

I've been following your thread with interest! I've started to look a little bit into the Kalman filter. Right now the problem is the gyro axis is orthogonal to the accelerometer axes, so there is nothing to correct the gyro with, unless I add a compass as well... So without the "merging" part, the Kalman filter is not much more useful than a simple high-low pass filter... Saying that, it might be possible to bring some of the accelerometer data in for correction when the tilt or roll of the head is large enough, so I might need to see how you did it in arduino code sometime!

Also I think your PPM reading thing will be useful too, for my own autopilot/OSD thing!

Thanks,
-Z-

Thank you to Zitron for the inspiration for my project, from which I intend to make the head-tracker, air mouse, and mini-Segway (perhaps transferred to fullsize DIY Segway our FIRST Robotics team is building).

I am using the Sparkfun Freeduino and Prototype shield, and from Pololu the inexpensive LISY300AL gyro (300 degrees/second) and MMA7260QT 3-axis accelerometer breakout boards which have integral regulators, so can be powered directly from the Arduino 5V supply.

I started my code based on the Arduino sample ADXL3xx, and have it working. The outputs are filtered using the algorithm from the DIY Segway project, it appears to work well.

Offsets are calculated at runtime, using the average of the first 500 samples. Once the offsets have been calculated, the program outputs them at regular intervals in a comma-separated string.

  if (millis_time >= millis_88Hz) {
    millis_88Hz = millis_time + freq_88Hz;
    
    rraw = analogRead(rpin);
    
    if (offsetreadings > 0) {
      rtot = rtot + rraw;                // long-term calculation of offset
      rtot_count++;
    }
    else if (offsetreadings == 0) {
      roff = int(rtot / rtot_count);     // permanent offset based on average
    }
    else {
       rread = rraw - roff;
       rfilt = rfilt * 0.9 + float(rread) * 0.1;
    }
  }

At first I was just reading the analog inputs as fast as possible, but then got reading the datasheets which state that gyro updates are at 88 Hz on the chip, and accelerometer outputs at 350Hz and 100 Hz for the z-axis. So there are separate 88, 100, and 350 Hz timers to read each input at the rates mentioned on the datasheets. Is this overkill? Am I misunderstanding what the datasheets are saying?

  if (millis_time >= millis_88Hz) {
    millis_88Hz = millis_time + freq_88Hz;
    
    rraw = analogRead(rpin);

    if (offsetreadings > 0) {
      ztot = ztot + zraw;                // long-term calculation of offset - not accurate enough
      ztot_count++;
    }
    else if (offsetreadings == 0) {
      zoff = int(ztot / ztot_count);     // permanent offset based on average
    }
  }

Now I'm going back over Zitron's code looking at the filter algorithm, and noticed this statement:

analogReference(EXTERNAL);

I have noticed that the offsets are calculated differently, each time I reset the Arduino, and sometimes not quite well enough. You can see the effect on the chart, the XYZ values should be centered on the zero axis but are slightly above.

I've wondered whether there is some voltage floating somewhere to cause this. I'm a programmer and ALWAYS suspect the electronics :wink: NEVER my (current!) understanding of the problem.

Anyway, I am hoping the analogReference() function will provide more consistent results. But looking at the Arduino page about analogReference, there is mention of using a 5K resistor to connect voltage inputs to the Arduino. I have been using straight wiring directly from the Pololu breakout board outputs to the analog in pins, I guess it's time to replace these with 5K resistors - before I add a analogReference(INTERNAL) line to my code?

Next I will be using Zitron's code to convert the filtered unit-less values I am outputting now, to degrees, Elevation, and Roll. Maybe someone can advise what to do with the Z-axis value ... at the moment it is only telling me whether the project is upside down or not.

Another issue - how to separate Elevation and Roll from the 'impulse acceleration' caused by motion? I would like to provide separate outputs to the computer for these, and have been trying to do this in the Visual Basic program with little success.

Mike

Here are a couple of photos and a chart to go with my previous post.




Mike

Hi Mike,

I've not seen your data sheet, but I think those output frequencies are usually the maximum frequencies the sensors can be use to detect. If your thing is vibrating at higher than 88Hz, the gyro will not be able to respond fast enough to output the true motion. So there it is probably better to try to sample at lower frequencies than the maximum.

I am sampling everything at 100Hz, which I think is more than enough, you should be able to get away sampling everything at 50Hz.

The analogReference(EXTERNAL) is used to map the voltage from the sensor to the analogRead() function for increased resolution. Because my arduino is at 5V, without the external reference, analogRead returns 1024 for 5V, 0 for 0V, but the maximum output of my sensors are 3V, so I'm only using 3/5th of the full resolution, by sending 3V to the analogue reference pin, and setting the analogReference(EXTERNAL), the voltages are mapped: 0V -> 0, 3V -> 1024.

Now, the 5K resistor is use to connect the external voltage to the analogue reference pin, not the analogue in pins!

Next I will be using Zitron's code to convert the filtered unit-less values I am outputting now, to degrees, Elevation, and Roll. Maybe someone can advise what to do with the Z-axis value ... at the moment it is only telling me whether the project is upside down or not.

You can use the arctan2(x,y) function like in my code to combine the Z value with X or Y, this way you can get a full +/-180 dgr values, with better accuracy.

Another issue - how to separate Elevation and Roll from the 'impulse acceleration' caused by motion? I would like to provide separate outputs to the computer for these, and have been trying to do this in the Visual Basic program with little success.

If the accelerometer is all you have, you can't! If you have the gyro working, you can use some sophisticated mathematical filters to reduce the effect of motion by combining the gyro and accelerometer readings. I am in the process of trying to do this myself...

Cool project, good luck!
-Z-