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

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

``````

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.

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

...

z >>= 2;
}
``````

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

``````

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

return;
``````
``````  readAccel();
``````

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

return;
``````

No, it will become:

``````void loop() {

timer = millis(); //get a start value to determine the time the loop takes