Go Down

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

zitron

Jan 05, 2009, 09:01 pm Last Edit: Jan 05, 2009, 09:55 pm by zitron Reason: 1
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:
http://www.rcgroups.com/forums/showthread.php?t=964184)



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:

http://vimeo.com/2590122


zitron

#1
Jan 05, 2009, 09:13 pm Last Edit: Jan 05, 2009, 09:16 pm by zitron Reason: 1
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.

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

}

Rick S.

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!

zitron

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-

Rick S.

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

Thanks for the inspiration though, this is neat!

Go Up