Go Down

Topic: Arduino 3DOF Head Tracker (Read 19487 times) previous topic - next topic

zitron

#15
Jan 23, 2009, 11:12 pm Last Edit: Jan 23, 2009, 11:12 pm by zitron Reason: 1
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-

zitron

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

http://vimeo.com/2939439

zitron

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


Code: [Select]
#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);  
 }

}

jimnaz

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

zitron

Quote
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-


socoj2

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

just wondering about wayts to simplify.

Ed Simmons

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

zitron

#22
Feb 03, 2009, 07:17 pm Last Edit: Feb 03, 2009, 07:18 pm by zitron Reason: 1
Quote
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.

Quote
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-

MLuckham

#23
Feb 08, 2009, 06:53 pm Last Edit: Feb 08, 2009, 07:01 pm by mluckham Reason: 1
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.

Code: [Select]

 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?

Code: [Select]

 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:

Code: [Select]

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


MLuckham

#24
Feb 08, 2009, 06:53 pm Last Edit: Feb 08, 2009, 06:54 pm by mluckham Reason: 1
Here are a couple of photos and a chart to go with my previous post.

http://www.sunsys.net/arduino_images/DSCF4609_small.JPG

http://www.sunsys.net/arduino_images/DSCF4610_small.JPG

http://www.sunsys.net/arduino_images/chart10.JPG

Mike

zitron

#25
Feb 10, 2009, 11:23 pm Last Edit: Feb 10, 2009, 11:24 pm by zitron Reason: 1
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!

Quote
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.

Quote

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-

zitron

Oh, by the way, if you need a way of plotting the data in real time, I'm written a program that takes a line of comma separated serial data and plot it:

http://iluvtocnc.googlepages.com/DataPlot.zip

Right now it's hard coded to use baud rate of 57600. It plots 4 channels at the same time. The format of the data is:

channel1,channel2,channel3,channel4\n

If you have less than 4 channels you need to use place holders like:

channel1,0,0,0\n

Cheers,
-Z-

MLuckham

Thanks Z.

That 5K resistor thing was really nagging me, it didn't make sense and also I feared it might mess with the RC filter already on the gyro/accelerometer outputs.  So after pondering the situation today and re-reading everything again, twice - I did discover that the resistor is to be added to the VREF pin, not to every input pin.  I didn't have a 5K, so I used a 4.7K and wired it up to the 3.3V produced by the Arduino's own regulator.  The ones on the gyro and accel boards seem to be more accurate, but they aren't the same and I couldn't decide which to use:

   Arduino 3.345V (3.266 mV/unit)
   Gyro 3.305V (3.227 mV/unit)
   Accelerometer 3.293V (3.215 mV/unit)

Now that the thing has been running with analogReference(EXTERNAL) without magic smoke, I'll probably use the gyro 3.3V output as it's the closest to ideal.

I've been using Hyperterminal to collect the CSV and Excel to chart, and wrote a VB program as well - it doesn't graph, but does use slider controls so you can see what is going on, and I'm playing around with drawing lines on a PictureBox to visualize how the sensor is oriented.  That will be easier once the code is outputting proper angles.

Offsets -  ::) hmmph - of course it was my code that calculated them wrongly (far too few samples), I changed my too-clever "offsetcount" method to a time measurement calibration instead ... I've tried 1, 5 and 10 seconds it doesn't seem to make much difference to the offsets, which pretty much take all the sensors to zero with no-motion - aside from the gyro drift of course.

Thanks for your insight about responsiveness - I removed my special timer code and the results were pretty well the same as before.

As for the 'impulse' acceleration and separating that from the physical position of the device, I've been playing around in the VB program and taking the difference between successive readings as the impulse, and trying to subtract an accumulation of impulses from the current value.

MLuckham

Do you know how to post images?

zitron

You have to host the file on some other server, and use the image tag...


Go Up