Need Help about Quaternion Algorithm Code

Hi, everyone! I want to make 3D positioning (Pitch, roll, and yaw) using this board: https://www.hobbyking.com/hobbyking/store/__27033__MultiWii_328P_Flight_Controller_w_FTDI_DSM2_Port.html And then i will use the positioning data to my 3 axis gimbal.

From the results of my search on the internet. I get the hint that one of the best methods for 3D positioning is quaternion.

And I found the Quaternion based algorithm from here: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/

It's open source, and i tried to use that algorithm on my code. This is my coomplete Code: https://github.com/mamette/quaternion/blob/master/AHRS.ino

I got the Pitch, Roll and Yaw value, but they are irregularly. Is anything wrong with my code?

And i have some question: 1. When we insert the Accelerometer value to Quaternion, the Accelerometer value must be in "g" scale or just raw value on each axis? 2. When we insert the Gyroscope value to Quaternion, the Gyroscope value must be in rad/s, it is right? 3. When we insert the Magnetometer value to Quaternion, just insert Magnetometer raw value on each axis, it is right?

Thank You..

byte readAccel()
{
  Wire.beginTransmission(BMA180);
  Wire.write(0x02);
  if(Wire.endTransmission()){return(1);}
  if(Wire.requestFrom(BMA180,7) != 7){return(2);}

...

  z >>= 2;
  temp = Wire.read();
}

First, since you are returning non-zero for errors, you should return zero at the very end (success).

Next:

void loop() {
  
  timer = millis(); //get a start value to determine the time the loop takes
  
  readCompass();
  getGyroscopeReadings(gyroResult);
  readAccel();

You aren't checking the return value. You may have had a transmission error. How about:

  if (readAccel() != 0)
    return;

And i have some question: 1. When we insert the Accelerometer value to Quaternion, the Accelerometer value must be in "g" scale or just raw value on each axis? 2. When we insert the Gyroscope value to Quaternion, the Gyroscope value must be in rad/s, it is right? 3. When we insert the Magnetometer value to Quaternion, just insert Magnetometer raw value on each axis, it is right?

Thank You..

The gyroscope value must be in radians, because the algorithm is using the cosine and sine functions somewhere, and they need radians to work properly.

The accelerometer and magnetometer values are effectively vectors indicating the direction of the gravitational and magnetic fields, the direction is what is important, the magnitude not so much.

[quote author=Nick Gammon link=topic=154901.msg1161177#msg1161177 date=1363584171]

byte readAccel()
{
  Wire.beginTransmission(BMA180);
  Wire.write(0x02);
  if(Wire.endTransmission()){return(1);}
  if(Wire.requestFrom(BMA180,7) != 7){return(2);}

...

  z >>= 2;
  temp = Wire.read();
}

First, since you are returning non-zero for errors, you should return zero at the very end (success). [/quote]

You mean like this:

...

  z >>= 2;
  temp = Wire.read();
return 0;

[quote author=Nick Gammon link=topic=154901.msg1161177#msg1161177 date=1363584171] Next:

void loop() {
  
  timer = millis(); //get a start value to determine the time the loop takes
  
  readCompass();
  getGyroscopeReadings(gyroResult);
  readAccel();

You aren't checking the return value. You may have had a transmission error. How about:

  if (readAccel() != 0)
    return;

[/quote]

So, it will become like this?

void loop() {
  
  timer = millis(); //get a start value to determine the time the loop takes
  
  readCompass();
  getGyroscopeReadings(gyroResult);
  readAccel();
if (readAccel() != 0)
    return;
  readAccel();
if (readAccel() != 0)

Why call readAccel() twice?

mamette: You mean like this:

...

  z >>= 2;   temp = Wire.read(); return 0;

Yes.

mamette: So, it will become like this?

void loop() {
  
  timer = millis(); //get a start value to determine the time the loop takes
  
  readCompass();
  getGyroscopeReadings(gyroResult);
  readAccel();
if (readAccel() != 0)
    return;

No, it will become:

void loop() {
  
  timer = millis(); //get a start value to determine the time the loop takes
  
  readCompass();
  getGyroscopeReadings(gyroResult);
  if (readAccel() != 0)
     return;

You can call a function and test its return value in the same statement.

millis() may not be fine enough resolution to get reliable behaviour - you could use micros() and scale it down a bit?

millis() for instance can sometimes skip by 2. micros() should be precise to 4us (but only as accurate as your crystal or resonator). Of course every way to determin time will experience some jitter due to the timer0 interrupt routine running, but that's perhaps 10 to 20us or so,