I am building a quadcopter using the Arduino Uno with a 6dof accelerometer and gyro. I will be adding a separate 3 axis magnetometer soon for heading. I have successfully implemented the code that reads the data coming in from these sensors and prints them out.
I have also checked for bias by averaging 100 measurements. My code calculates the pitch from the accel and pitch from the gyro respectively:
pitchAccel = atan2((accelResult[1] - biasAccelY) / 256, (accelResult[2] - biasAccelZ) / 256)*180.0 / (PI);
pitchGyro +=((gyroResult[0] - biasGyroX) / 14.375)*dt;
I am then using a complementary filter to fuse the two readings together like this:
pitchComp = (0.98*pitchGyro) + (pitchAccel*0.02);
I am stuck on how to proceed from here. I am using the same procedure for roll, so I now have readings for pitch and roll from their respective complementary filter outputs.
I have read a lot of articles on the DCM algorithm which relates the angles from the body reference frame to the earth reference frame. Should that be my next step here? Taking the pitch and roll readings in the body reference frame and transforming them to the earth reference frame? Repeat the entire procedure for yaw using the magnetometer? If yes, how should I go about doing the transformations?
I understand the math behind it, but I am having a hard time understanding the actual implementation of the DCM algorithm code-wise.
Any help is appreciated!