Guide to gyro and accelerometer with Arduino including Kalman filtering

Hey :slight_smile:

I have a continues Servo... and after doing all of this, my gyro is still like 95% accurate ...
i need 99% atleast..

I just think i'm gonna get an Absolut Angle sensor and connect it to the servo..

Thanks for the help anyway, you were very nice and helpfull :slight_smile:

Why not just use a ordinary Servo then?
Thank you for the kinds words :slight_smile:

Regards
Lauszus

Hello,

I am using an Arduino UNO and a Sparkfun 6 DOF Razor IMU. I am hoping to adapt it to control roll and pitch for a quadrotor stabilization system. I used your post as a guide to setup the wiring and used the version 3 code to get it going. My problem is I get about 2-3 lines of actual data and then it just scrolls jibberish. I double checked the wiring and I have not changed anything in the code. Could it be that I am using the IMU Analog Combo Board Razor - 6DOF Ultra-Thin IMU - SEN-10010 - SparkFun Electronics IMU with the high pass filters removed.

I would like to apologize in advance, I have a lot of RC experience and a mechanical engineering background but programming and debug has never been my forte.

All I want to do is read in the roll and pitch degrees and then convert that to a servo command to send to a brushless motor as an increase in thrust to compensate.

Thanks for any help you can provide.

Stefan

It is exactly the same IMU as mine. So it should be almost plug n' play. Triple check your wires, or try resetting the high pass filter, by pulling the HP pin high (see the datasheet page 7 http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf). Not sure if it would work, as the description says that it it removed, as you said.

You do not have to apologize :slight_smile: We have all been noobs once :smiley:

Regards
Lauszus

@StefanElsener
I also have the board with sensors removed and the program works great, if you say that you can read a few lines and then it just scrolls jibberish it might be the baud rate. 98% of the time that i see just jibberish on the Serial monitor is because the baud rate in it is not the same as the one on the program check that the baud rate on the script is the same as in the serial monitor (bottom right). If that doesnt work then do as Lauszus says check your wires, maybe download the program again and make sure not to erase anything important on it. If that doesnt work then im sorry to say that your board might be damaged (its painful i know :frowning: ...) good luck

Thank You Cristo! I had the baud rate set at 9600 baud (default setting I assume as this is the first time I have used the program) and when I changed it to 115,200 baud it worked!!

Thank you Lauszus for the quick reply. Now for my next task, creating the control function to provide stabilization for a quadcopter.... does anybody have a good starting point now that my IMU is working?

Thanks again!

Stefan

In fact im doing the exact same thing, although i gotta warn you this sensor might not be the best choise it is too sensitive to vibrations and its not the fastest one there is but if you wana try it join the club lol, also i think is better to use DCM rather than Kalman unless you use expanded Kalman or something like that so you can go the 360. I hope you are taking someone elses program Aeroquad or Multiwiii as your program creating your own takes a long long time and it will never work as good as their programs. I had to do it since it is a project for school but if you like programing and that sort of stuff try using one of those programs which are already working and just add some crazy stuff to make it automated like a GPS where you can set the route or stuff like that.

If you still feel like doing this then i can send you as many links as you want the first one is a DCM code for the same board we are using although i dont know why but i havent been able to make it work maybe you have more luck with it: voidbot.net if you have any luck let me know.

Also if you want to learn the basics on all the stuff you need to create the program take a look at my posts:

http://arduino.cc/forum/index.php/topic,74234.0.html

The DIY is better since there is a guy that answered all my questions.

Good luck and i hope you are making this with someone else otherwise is a lot of work If you are doing this as a hobby dont try it just use the programs there are you will enjoy it much more and save a lot of money building structures...

No, I would love to use existing program. I am not a good enough programmer to make one myself. I looked at the Aeroquad but it doesn't work for my board (Ardunio Uno). I know this won't be the best setup but right now I am just looking to make a basic IMU with Roll and Pitch stabilization. I can add better functionality later. Just want to play with this in my backyard maybe with an on board camera or something.

I remember those days when i thought it was that simple, i have 3 weeks to make this quadcopter fly and right know it only hovers i have a good idea of what might help it though. I hope ill get it this weekend.

The Aeroquad can use the UNO it just uses less sensors (accelerometer and gyroscope) just buy whatever they say and you will have it working
http://aeroquad.com/showwiki.php?title=Parts+List
In there you see that there are 3 different configurations one with the mega one with the uno and the other with the mini just follow the instructions and be careful.

If you dont want to use the aeroquad code just use the multiwii thats what almost everyone in RCgroups use also they have a nice one where you can use the wii control sensors could make this really cheap.

But if you want to be certain that it will work i would recomend aeroquad

PS i havent tried any of those programs personally, it would brake my heart to see my quad fly with another program.

Lauszus:
Why not just use a ordinary Servo then?
Thank you for the kinds words :slight_smile:

Regards
Lauszus

Hey Lauszuz,

Sorry for the long time till i replay i was away.

An ordinary Servo wont give me the Power i need, and i need 360 degrees and not 180, and i have size capabilities.

I think i will get the IMU that you use, will it give me 100% accuuracy ?

Okay :slight_smile: It is actually possible to get a servo that can rotate 360 degrees. I found this after some googling: Pololu - GWS S125 1T 2BB Sail Winch Servo , that might do the job.

The IMU I used can only get 360 degrees resolution, if you only use either pitch or roll, but it can not be used at the same time...
But if you only need one of them, the IMU will work just fine :smiley:

Regards Lauszus

Lauszus:
Okay :slight_smile: It is actually possible to get a servo that can rotate 360 degrees. I found this after some googling: Pololu - GWS S125 1T 2BB Sail Winch Servo , that might do the job.

The IMU I used can only get 360 degrees resolution, if you only use either pitch or roll, but it can not be used at the same time...
But if you only need one of them, the IMU will work just fine :smiley:

Regards Lauszus

Hey, Thanks !

I actually need a really strong Servo, i need something like 70kg/cm :slight_smile:
I found this http://www.hobbyking.com/hobbyking/store/uh_viewItem.asp?idProduct=18742 and i will add him another gear to make it 100gk/cm :slight_smile:

If i get the IMU that you use in your example and use the Kalman Filtering, will i get 100% accuracy on 360 rotation ?

Thanks,
Dan

If you have a servo you dont need the sensor to know the angle, you just tell the servo which angle you want with the servo library. If you need an even bigger motor then you could buy a motor decoder some of them give you the exact position of the motor i saw one once and it wasperfect i cant seem to find a link sorry.

@ Lauszus sorry i always interfere in your post it was of so much help and want to help as much as i can xD

I have a question im using different sensors the i2c ones Im able to get all the angles now X Y Z but when i go past 90 it starts going down like 87 88 89 89 87 or -87 -89 -89 -87 how could i get the full 180... Pythagoras was never my friend maybe you have some good ideas.

PS the Z axis is working fine it goes 187 188 189 -189 -188 -187 if i could get the same result on the other 2 angles it would be perfect

Thanks

Hi

what do i have to change to the program to use it with a 5 d-o-f imu?

I'm using a idg500/adxl335 IMU and an arduino uno

You only need to change the sensitivity for the gyro and accelerometer, since the gyro for the yaw axis is not used, you can still get the same result as shown in the demonstration video with a 5DOF :slight_smile:

Regards
Lauszus

Thank you for your reply, Lauszus.

we changed the values of the sensitivity as you recommended, but it doesn't seem to be working.
I'm not sure what does
float Q_angleY = 0.3;
float Q_gyroY = 0.002;
float R_angleY = 0.03;
refer to..

do they mean
gyro
accelerometer
force vector?

//made by Kristian Lauszus - see http://arduino.cc/forum/index.php/topic,58048.0.html for information
#define gX A0
#define gY A1

#define aX A3
#define aY A4
#define aZ A5

#define RAD_TO_DEG 57.295779513082320876798154814105

//gyros
int gyroZeroX;//x-axis
float gyroXadc;
float gyroXrate;
float gyroXangle;

int gyroZeroY;//y-axis
float gyroYadc;
float gyroYrate;
float gyroYangle;

//accelerometers
int accZeroX;//x-axis
float accXadc;
float accXval;
float accXangle;

int accZeroY;//y-axis
float accYadc;
float accYval;
float accYangle;

int accZeroZ;//z-axis
float accZadc;
float accZval;
float accZangle;

//Results
float xAngle;
float yAngle;
float compAngleX;
float compAngleY;

float R;//force vector
//Used for timing
unsigned long timer=0;
unsigned long dtime=0;

//This is pretty simple. It takes 100 readings and calculate the average.

//gyros
int inputGyroX[100];//x-axis
long resultGyroX;

int inputGyroY[100];//y-axis
long resultGyroY;

//accelerometers
int inputAccX[100];//x-axis
long resultAccX;

int inputAccY[100];//y-axis
long resultAccY;

int inputAccZ[100];//z-axis
long resultAccZ;

//gyros
int calibrateGyroX()
{
  for(int i=0;i<100;i++)
  {
    inputGyroX[i] = analogRead(gX);
  }
  for(int i=0;i<100;i++)
  {
    resultGyroX += inputGyroX[i];
  }
  resultGyroX = resultGyroX/100;
  return resultGyroX;
}

int calibrateGyroY()
{
  for(int i=0;i<100;i++)
  {
    inputGyroY[i] = analogRead(gY);
  }
  for(int i=0;i<100;i++)
  {
    resultGyroY += inputGyroY[i];
  }
  resultGyroY = resultGyroY/100;
  return resultGyroY;
}

//accelerometers
int calibrateAccX()
{
  for(int i=0;i<100;i++)
  {
    inputAccX[i] = analogRead(aX);
  }
  for(int i=0;i<100;i++)
  {
    resultAccX += inputAccX[i];
  }
  resultAccX = resultAccX/100;
  return resultAccX;
}
 
int calibrateAccY()
{
  for(int i=0;i<100;i++)
  {
    inputAccY[i] = analogRead(aY);
  }
  for(int i=0;i<100;i++)
  {
    resultAccY += inputAccY[i];
  }
  resultAccY = resultAccY/100;
  return resultAccY;
}

int calibrateAccZ()
{
  for(int i=0;i<100;i++)
  {
    inputAccZ[i] = analogRead(aZ);
  }
  for(int i=0;i<100;i++)
  {
    resultAccZ += inputAccZ[i];
  }
  resultAccZ = resultAccZ/100;
  return resultAccZ;
}

// KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleX  =  0.3; 
    float Q_gyroX   =  0.002;  
    float R_angleX  =  0.03;  

    float x_angle = 0;
    float x_bias = 0;
    float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;	
    float dtX, yX, SX;
    float KX_0, KX_1;

  float kalmanCalculateX(float newAngle, float newRate,int looptime) {
    dtX = float(looptime)/1000;                                    // XXXXXXX arevoir
    x_angle += dtX * (newRate - x_bias);
    PX_00 +=  - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
    PX_01 +=  - dtX * PX_11;
    PX_10 +=  - dtX * PX_11;
    PX_11 +=  + Q_gyroX * dtX;
    
    yX = newAngle - x_angle;
    SX = PX_00 + R_angleX;
    KX_0 = PX_00 / SX;
    KX_1 = PX_10 / SX;
    
    x_angle +=  KX_0 * yX;
    x_bias  +=  KX_1 * yX;
    PX_00 -= KX_0 * PX_00;
    PX_01 -= KX_0 * PX_01;
    PX_10 -= KX_1 * PX_00;
    PX_11 -= KX_1 * PX_01;
    
    return x_angle;
  }

 // KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleY  =  0.3; 
    float Q_gyroY   =  0.002;  
    float R_angleY  =  0.03;  

    float y_angle = 0;
    float y_bias = 0;
    float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;	
    float dtY, yY, SY;
    float KY_0, KY_1;

  float kalmanCalculateY(float newAngle, float newRate,int looptime) {
    dtY = float(looptime)/1000;                                    // XXXXXXX arevoir
    y_angle += dtY * (newRate - y_bias);
    PY_00 +=  - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
    PY_01 +=  - dtY * PY_11;
    PY_10 +=  - dtY * PY_11;
    PY_11 +=  + Q_gyroY * dtY;
    
    yY = newAngle - y_angle;
    SY = PY_00 + R_angleY;
    KY_0 = PY_00 / SY;
    KY_1 = PY_10 / SY;
    
    y_angle +=  KY_0 * yY;
    y_bias  +=  KY_1 * yY;
    PY_00 -= KY_0 * PY_00;
    PY_01 -= KY_0 * PY_01;
    PY_10 -= KY_1 * PY_00;
    PY_11 -= KY_1 * PY_01;
    
    return y_angle;
  }

 //print data to processing
void processing()
{
  Serial.print(gyroXangle);Serial.print("\t");
  Serial.print(gyroYangle);Serial.print("\t");
  
  Serial.print(accXangle);Serial.print("\t");
  Serial.print(accYangle);Serial.print("\t");  
  
  Serial.print(compAngleX);Serial.print("\t");  
  Serial.print(compAngleY); Serial.print("\t"); 
  
  Serial.print(xAngle);Serial.print("\t");
  Serial.print(yAngle);Serial.print("\t");
   
  Serial.print("\n");
  delay(1000);
} 



void setup()
{
  analogReference(EXTERNAL); //3.3V
  Serial.begin(9600);
  delay(100);//wait for the sensor to get ready
  
  //calibrate all sensors in horizontal position
  gyroZeroX = calibrateGyroX();
  gyroZeroY = calibrateGyroY();  
  
  accZeroX = calibrateAccX();
  accZeroY = calibrateAccY();
  accZeroZ = calibrateAccZ();
  
  timer=millis();//start timing
}

void loop()
{
  gyroXadc = analogRead(gX);
  gyroXrate = (gyroXadc-gyroZeroX)/1.0323;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  gyroXangle=gyroXangle+gyroXrate*dtime/1000;//Without any filter
  
  gyroYadc = analogRead(gY);
  gyroYrate = (gyroYadc-gyroZeroY)/1.0323;//(gyroYadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  gyroYangle=gyroYangle+gyroYrate*dtime/1000;//Without any filter
  
  accXadc = analogRead(aX);
  accXval = (accXadc-accZeroX)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  
  accYadc = analogRead(aY);
  accYval = (accYadc-accZeroY)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  
  accZadc = analogRead(aZ);
  accZval = (accZadc-accZeroZ)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  accZval++;//1g in horizontal position
  
  R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
  accXangle = acos(accXval/R)*RAD_TO_DEG-90;
  accYangle = acos(accYval/R)*RAD_TO_DEG-90;
  //accZangle = acos(accZval/R)*RAD_TO_DEG;
  
/*  //used for debugging
  Serial.print(gyroZrate,0);Serial.print("\t");
  Serial.print(gyroZrate,0);Serial.print("\t");
  Serial.print(gyroZrate,0);Serial.print("\t");

  Serial.print(gyroXangle,0);Serial.print("\t");
  Serial.print(gyroYangle,0);Serial.print("\t");
  Serial.print(gyroZangle,0);Serial.print("\t");

  Serial.print(accXval,2);Serial.print("\t");
  Serial.print(accYval,2);Serial.print("\t");
  Serial.print(accZval,2);Serial.print("\t");

  Serial.print(accXangle,0);Serial.print("\t");
  Serial.print(accYangle,0);Serial.print("\t");
  //Serial.print(accZangle,0);Serial.print("\t");
*/
  
  dtime = millis()-timer;
  timer = millis();
  
  compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
  compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
  xAngle = kalmanCalculateX(accXangle, gyroXrate, dtime);
  yAngle = kalmanCalculateY(accYangle, gyroYrate, dtime);

  /*Serial.print(compAngleX,0);Serial.print("\t");
  Serial.print(compAngleY,0);Serial.print("\t");
  Serial.print(xAngle,0);Serial.print("\t");
  Serial.print(yAngle,0);Serial.print("\t");
  Serial.println("");*/
  
  processing();  
}

That's the process and measurement noise constant used in the Kalman Filter. Do not change it!! At least until you got everything working. I recommend changing them back to there originals values:

float Q_angleX  =  0.001; 
float Q_gyroX   =  0.003;  
float R_angleX  =  0.03;

And then wait until you got it working.
See these links for more information: http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf.

You have to change the set the sensitivity in the code. Right now you are not setting the constant. For example in this line:

accXadc = analogRead(aX);
accXval = (accXadc-accZeroX)/102,3;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3

Send me a link to the datasheets and I will figure the sensitivity out for you :slight_smile:

Regards
Lauszus

Hi,
I will respond to your personal messages here, so everyone else can learn by your mistakes :slight_smile:
First you wrote:

Thank you for your reply.

How should i program it so? How do i integrate both the gyro and accelerometer into one program? i'm relatively new to arduino and have just started this project, i have totally no idea how to do it. I tried the gyro program in the arduino playground but the values do not seem to stop dropping..

your reply is very appreciated! sorry for my bad english

Just grab my program and change the sensitivity (see the next respond). The reason why you experience a drop, it's what called a drift. All gyros do that!! But the Kalman filter will take care of this :slight_smile:

The second message:

Hi,

I just saw your reply to my question in the post, here you go
idg500 http://www.sparkfun.com/datasheets/Components/SMD/Datasheet_IDG500.pdf
adxl335 http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf

your help is really much appreciated :slight_smile:

If your are using 3.3 volt, then your sensitivity would be:

gyroSensitivity: 0.0091/3.31023=2.821 (4.5 times output) else 0.002/3.31023=0.62 (normal output)
accelerometerSensitivity: 0.300/3.3*1023=93

Hope it make more sense to you now :slight_smile:

  • Lauszus

Thank you Lauszus for your prompt reply,

#define gX A0

#define aX A3

#define RAD_TO_DEG 57.295779513082320876798154814105

//gyros
int gyroZeroX;//x-axis
float gyroXadc;
float gyroXrate;
float gyroXangle;

//accelerometers
int accZeroX;//x-axis
float accXadc;
float accXval;
float accXangle;

//Results
float xAngle;
float yAngle;
float compAngleX;
float compAngleY;

float R;//force vector
//Used for timing
unsigned long timer=0;
unsigned long dtime=0;

//This is pretty simple. It takes 100 readings and calculate the average.

//gyros
int inputGyroX[100];//x-axis
long resultGyroX;

//accelerometers
int inputAccX[100];//x-axis
long resultAccX;

//gyros
int calibrateGyroX()
{
  for(int i=0;i<100;i++)
  {
    inputGyroX[i] = analogRead(gX);
  }
  for(int i=0;i<100;i++)
  {
    resultGyroX += inputGyroX[i];
  }
  resultGyroX = resultGyroX/100;
  return resultGyroX;
}

//accelerometers
int calibrateAccX()
{
  for(int i=0;i<100;i++)
  {
    inputAccX[i] = analogRead(aX);
  }
  for(int i=0;i<100;i++)
  {
    resultAccX += inputAccX[i];
  }
  resultAccX = resultAccX/100;
  return resultAccX;
}

// KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleX  =  0.001; 
    float Q_gyroX   =  0.003;  
    float R_angleX  =  0.03;  

    float x_angle = 0;
    float x_bias = 0;
    float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;	
    float dtX, yX, SX;
    float KX_0, KX_1;

  float kalmanCalculateX(float newAngle, float newRate,int looptime) {
    dtX = float(looptime)/1000;                                    // XXXXXXX arevoir
    x_angle += dtX * (newRate - x_bias);
    PX_00 +=  - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
    PX_01 +=  - dtX * PX_11;
    PX_10 +=  - dtX * PX_11;
    PX_11 +=  + Q_gyroX * dtX;
    
    yX = newAngle - x_angle;
    SX = PX_00 + R_angleX;
    KX_0 = PX_00 / SX;
    KX_1 = PX_10 / SX;
    
    x_angle +=  KX_0 * yX;
    x_bias  +=  KX_1 * yX;
    PX_00 -= KX_0 * PX_00;
    PX_01 -= KX_0 * PX_01;
    PX_10 -= KX_1 * PX_00;
    PX_11 -= KX_1 * PX_01;
    
    return x_angle;
  }

 // KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
//with slightly modifications by Kristian Lauszus

    float Q_angleY  =  0.001; 
    float Q_gyroY   =  0.003;  
    float R_angleY  =  0.03;  

    float y_angle = 0;
    float y_bias = 0;
    float PY_00 = 0, PY_01 = 0, PY_10 = 0, PY_11 = 0;	
    float dtY, yY, SY;
    float KY_0, KY_1;

  float kalmanCalculateY(float newAngle, float newRate,int looptime) {
    dtY = float(looptime)/1000;                                    // XXXXXXX arevoir
    y_angle += dtY * (newRate - y_bias);
    PY_00 +=  - dtY * (PY_10 + PY_01) + Q_angleY * dtY;
    PY_01 +=  - dtY * PY_11;
    PY_10 +=  - dtY * PY_11;
    PY_11 +=  + Q_gyroY * dtY;
    
    yY = newAngle - y_angle;
    SY = PY_00 + R_angleY;
    KY_0 = PY_00 / SY;
    KY_1 = PY_10 / SY;
    
    y_angle +=  KY_0 * yY;
    y_bias  +=  KY_1 * yY;
    PY_00 -= KY_0 * PY_00;
    PY_01 -= KY_0 * PY_01;
    PY_10 -= KY_1 * PY_00;
    PY_11 -= KY_1 * PY_01;
    
    return y_angle;
  }

 //print data to processing
void processing()
{
  Serial.print(gyroXangle);Serial.print("\t");
   
  Serial.print(accXangle);Serial.print("\t");
  
  //Serial.print(compAngleX);Serial.print("\t");  
  //Serial.print(compAngleY); Serial.print("\t"); 
  
  Serial.print(xAngle);Serial.print("\t");
  //Serial.print(yAngle);Serial.print("\t");
   
  Serial.print("\n");
  delay(1000);
} 



void setup()
{
  analogReference(EXTERNAL); //3.3V
  Serial.begin(9600);
  delay(100);//wait for the sensor to get ready
  
  //calibrate all sensors in horizontal position
  gyroZeroX = calibrateGyroX();
  
  accZeroX = calibrateAccX();
  
  timer=millis();//start timing
}

void loop()
{
  gyroXadc = analogRead(gX);
  gyroXrate = (gyroXadc-gyroZeroX)/0.62;//(gyroXadc-gryoZeroX)/Sensitivity - in quids              Sensitivity = 0.00333/3.3*1023=1.0323
  gyroXangle=gyroXangle+gyroXrate*dtime/1000;//Without any filter

  
  accXadc = analogRead(aX);
  accXval = (accXadc-accZeroX)/93;//(accXadc-accZeroX)/Sensitivity - in quids              Sensitivity = 0.33/3.3*1023=102,3
  
  
  //R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
  accXangle = acos(accXval/R)*RAD_TO_DEG-90;
  //accYangle = acos(accYval/R)*RAD_TO_DEG-90;
  //accZangle = acos(accZval/R)*RAD_TO_DEG;
  
/*  //used for debugging
  Serial.print(gyroZrate,0);Serial.print("\t");
  Serial.print(gyroZrate,0);Serial.print("\t");
  Serial.print(gyroZrate,0);Serial.print("\t");

  Serial.print(gyroXangle,0);Serial.print("\t");
  Serial.print(gyroYangle,0);Serial.print("\t");
  Serial.print(gyroZangle,0);Serial.print("\t");

  Serial.print(accXval,2);Serial.print("\t");
  Serial.print(accYval,2);Serial.print("\t");
  Serial.print(accZval,2);Serial.print("\t");

  Serial.print(accXangle,0);Serial.print("\t");
  Serial.print(accYangle,0);Serial.print("\t");
  //Serial.print(accZangle,0);Serial.print("\t");
*/
  
  dtime = millis()-timer;
  timer = millis();
  
  compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
  //compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
  xAngle = kalmanCalculateX(accXangle, gyroXrate, dtime);
  //yAngle = kalmanCalculateY(accYangle, gyroYrate, dtime);

  /*Serial.print(compAngleX,0);Serial.print("\t");
  Serial.print(compAngleY,0);Serial.print("\t");
  Serial.print(xAngle,0);Serial.print("\t");
  Serial.print(yAngle,0);Serial.print("\t");
  Serial.println("");*/
  
  processing();  
}

we require vertical tilt detection, does it matter which accelerometer we use? we are currently using the x gyro and x accelerometer...
and after we turn the gyro 360 degrees it only shows 180 degrees rotation, i think we are doing something wrong, could you please advice :smiley:

your reply is very much appreciated!

First of all. You can't calculate the force vector with only 1 accelerometer axis. You need 3! You are dividing with a unset variable.
Instead your should write:

accXangle = asin(accXval)*RAD_TO_DEG;

But if you use all 3-axis, then you will get better precision, so I recommended that!!
So connect all three axis and calculate all the values as so:

accXadc = analogRead(aX);
accXval = (accXadc-accZeroX)/93;

accYadc = analogRead(aY);
accYval = (accYadc-accZeroY)/93;

accZadc = analogRead(aZ);
accZval = (accZadc-accZeroZ)/93;
  
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//the force vector
accXangle = acos(accXval/R)*RAD_TO_DEG-90;

You can only use the x and y axis.

You can get 360 resolution by using the z-axis. Take a look in the version 3 of the code. This is what I did:

  if(accZval < 0)//360 degrees resolution
  {
    if(accXangle < 0)
    {
      accXangle = -180-accXangle;
    }
    else
    {
      accXangle = 180-accXangle;
    }        
  }
  • Lauszus