next topic »

[on:]

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:

[Reply #1 on:]

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:

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

}

[Reply #2 on:]

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!

[Reply #3 on:]

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-

[Reply #4 on:]

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!

[Reply #5 on:]

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

[Reply #6 on:]

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.

[Reply #7 on:]

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(;   ????   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

[Reply #8 on:]

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( takes the average of 8 readings. These are just raw A2D readings.

Code:

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

gRaw is the raw gyro value*10, 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 stationary*10, 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.

Code:

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-

[Reply #9 on:]

Thank you, i really appreciate your help.

[Reply #10 on:]

Check out the Angle complementary filter on this site: http://web.mit.edu/first/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.

[Reply #11 on:]

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

-Z-

[Reply #12 on:]

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

[Reply #13 on:]

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-

[Reply #14 on:]

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