We are trying to implement Mayhony & Madgwick IMU filter algorithm on the Arduino megaboard 2560 (we tried both filters). We got the algorithm from this site: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
So far, we pretty much copy pasted the algorithm onto the board. We are using the Oilpan IMU (its a 6 axis with gyro and accelerometer w/o magnetometer). Also, we tried adjusting the gyromeasurement definition in the code but the results didn't improve.
We are getting readings from the board but they seem to drift over time. For example, when we pitch the board the results show some value. This value then starts to decrease to the negative of the value and then increases again. It keeps oscillating like this. In certain cases, however, this doesn't happen and the value remains stable.
Another problem is that when we pitch the board we get both pitch and roll readings and vice versa with the roll. So for instance, if we pitch for 20 degrees we get some value for pitch and some value for roll (and then the drift problem comes in with both).
Our code is as follows:
//Read Sensors Voltage levels
AZ = Read_ADC(3)*(3.3/4095);
AY = Read_ADC(6)*(3.3/4095);
AX = Read_ADC(2)*(3.3/4095);
AccelZ = (AZ-1.66) * 3.3333 * g; // Calculate Acceleration in Z
AccelY = (AY-1.66) * 3.3333 * g; // Calculate Acceleration in Y
AccelX = (AX-1.66) * 3.3333 * g;
GZ = (Read_ADC(0)*(3.3/4095)) - 1.35; // Read Gyro sensor
GX = (Read_ADC(4)*(3.3/4095)) - 1.36;
GY = (Read_ADC(1)*(3.3/4095)) - 1.35;
GyroRateZ = (GZ * 500)* PI/180; // Convert to rads/s
GyroRateX = (GX * 500)* PI/180;
GyroRateY = (GY * 500)* PI/180;
// The data is then sent through a serial port to Matlab for plotting
Serial.println("B"); // Indicate Beginning of data
Thanks in advance!