Regarding the MPU-6050 chips what package are you interfacing with, MPU-6050 Sensor Board for Atmel AVR UC3 ($54.00) or the MPU-6050EVB ($69.95). I'm thinking the $ might ring a bell:) Or did you just get the chip?
I finally got my sensor stick from sparkfun 9DOF (ITG3200, ADXL345 and HMC5883) and managed to run your DCM code. After a few axis swap it works really well with your PC visualiser.
How can I get Euler Angles from the Matrix. Any help would be great.
As I see, yaw angle is pretty stable with only gyro/accel, but any chance to integrate magnetometer ?
I've been trying to get the DCM to work with my setup, on my parallax serial terminal I've been seeing a bunch of random characters. I've had to change the ITG-3200 address and I'm using a 6 mhz crystal (96,000,000 MHz) and I think I've adjusted the code for my setup.
I was able to get my adxl345 working but i've been having issues with the output of the itg-3200 and the rotation matrix. Could someone take a screen shot of the correct output so that I can have a basis to go off on? thanks
Are you trying to use the code as posted? It outputs raw binary data of the current orientation matrix, intended for the program that draws the cube. It's not ASCII, and not human readable. A matrix isn't particularly intuitive if you're just looking at the raw numbers, which is why it's done this way.
Eser- getting angles from the matrix requires doing an asin() on two of the components of the matrix - one for pitch, the other for roll, and heading is computed by doing an atan() of two more of the components. I can't remember which two, so I'll have to get back to you.
It is also possible (actually not very hard) to add a compass into the mix. I have a compass that I'm going to try it with.
I finally got my sensor stick from sparkfun 9DOF (ITG3200, ADXL345 and HMC5883) and managed to run your DCM code. After a few axis swap it works really well with your PC visualiser.
How can I get Euler Angles from the Matrix. Any help would be great.
As I see, yaw angle is pretty stable with only gyro/accel, but any chance to integrate magnetometer ?
I have tried to convert DCM to Euler Angles. I have used below equations and fixed point math trigonometric functions (http://obex.parallax.com/objects/501/)
but It was too slow. Even running in a separate jog.
Aydin I too wondered how Jason would determine pitch,roll, and yaw with the Propeller from the DCM matrix. I tried your formula above (with no luck) using float32full.spin, floatstring.spin to display the output to the serial terminal with Jason's code with this inserted.
DCM Matrix is in Fixed-point format 15bit whole part 16 bit fraction part 1 bit sign, because of that we cant use FFloat() function. It converts from Integer to float.
I have derived conversion to Euler formulas above from Ardupilot project.
DCM update rate is 200Hz in Jason's code. This is 5 milisec. (400.000 clockcycle) DCM algorithm is using approximately half of it.
Then we have approx 200.000 clocksycle or 2.5 milisec for conversion. Or conversion can be done in separate cog.
We need a fast way to convert fixed-point to float.
I have code that does this already, but I'm at work at the moment, so I don't have access to it. I do this using the float32 library, but you need to use Float32Full. I found a modified Float32 that moved atan2, acos, and asin into the main cog, replacing log and pow, because those weren't used in my code. This takes the requirements from 2 cogs back down to 1 cog.
To do the conversion, the simplest way is to convert the full fixed point number to float, then divide by (float)(1 << FixedBits), whatever that ends up being.
myFloat := f32.fdiv( f32.ffloat( FixedPointValue ) , 16384.0 ) '16384 = 1<<14, which I think is what I'm using for FixedBits
There's also a much faster way to do it. Floating point numbers are represented by a sign bit, an exponent, and a mantissa, and work essentially like this:
(sign) x (mantissa) x 2 to the power of (exponent)
The sign is a single bit, the mantissa is 23 bits, and the exponent is 8 bits. If you need to multiply or divide any floating point number by a power of two, you can simply add or subtract the power to/from the exponent. It's the equivalent of shifting a regular integer. So, to divide a non-zero float by (1 << FixedBits), just use plain integer math to subtract (FixedBits) from the exponent. The only slightly tricky bit is that you have to make sure it isn't zero first or you'll mess it up. So, it looks like this:
myFloat := f32.ffloat( FixedPointValue )
if( myFloat <> 0 ) 'check to make sure the float is not zero (zero looks the same in int or float)
myFloat -= FixedBits 'divide the float value by (1 << FixedBits)
I went a step further and implemented a function that does this in my floating point cog, so it takes an extra two instructions instead of an extra few hundred cycles.
These are the lines I use in my current version (I've altered my version of the code to use 28 fixed bits instead of 14)
rx := Flt.ASin( Flt.FFloatDiv28( DCM.GetM12 ) ) 'Convert to float and divide by (float)(1<<28), then take ArcSine
rz := Flt.ASin( Flt.FFloatDiv28( DCM.GetM10 ) ) 'Convert to float and divide by (float)(1<<28), then take ArcSine
ry := Flt.Atan2( Flt.FFloat(DCM.GetM20), Flt.FFloat(DCM.GetM22) )
You don't need to scale down if you're using Atan2 because it doesn't care what the range of inputs is - it's just converting from a coordinate pair to an angle. ArcSin requires the inputs be in the range of -1 to 1, so I scale them down by FixedBits before doing those conversion.
The results are independent of each other - rx = amount of pitch around the X axis, ry = amount of roll around the Y axis, and rz = heading. This code does not handle what happens when the craft is inverted.
I just double checked my original code - The FixedBits value is 15, not 14, so the "16384" in your code should be "32768". That might be all that's required to fix it, so give that a try and let me know.
Not sure if it's helpful for this problem specifically but we post a lot of resources for calibrating, and working with IMU at http://www.chrobotics.com/shop/orientation-sensor-um6 under the "Resources" tab. Anything from documentation to software downloads, firmware updates and the source code for the UM6 Orientation Sensors we sell. You're welcome to read our material and see if it would be helpful in solving a problem with other IMUs.
hi jasons
many thanks for the Video and the DCM-Visualizer..... :-))
i have this done on Linux and opengl , but it was the wrong way .... i have working with a 3D-Quadcopter model, so it was too slow.
your video gives me a new way thanks
in the last weeks i work on a new QuadCopter Body with C3 (LaserRangeFinder, GPS, XBEE, and intercommunications to my PPB as master)
and the PPB ( L3G4200D (3AxisGyroscopte) PID , 6channel-RC-Receiver and 4-channel ServoPort-ESC)
I find your DCM - code verry cool
thanks for sharing
regards nomad
Hi jason,
Do you have any progress on your DCM code. Did you ever try it on a fixed-wing platform. I mean effects of centripetal acceleration.
Did you have a version of DCM code with euler angles output? Because I have tried to convert euler angles from DCM, but I failed.
I have a faster version of the DCM code that also maintains more bits of precision, and I also have code that extracts the roll and pitch angles from the matrix.
I'll have to clean it up a bit before I can post the whole thing, but essentially the DCM to angles conversion is this:
Roll := Flt.ASin( Flt.FFloatDiv28( DCM.GetM12 ) ) 'Convert to float, then divide by (float)(1<<28)
Pitch := Flt.ASin( Flt.FFloatDiv28( DCM.GetM10 ) ) 'Convert to float, then divide by (float)(1<<28)
The new version of the DCM code maintains 28 bits of integer precision, so AFTER converting to float, I have to scale the numbers down by 1<<28 to bring them into the range of -1.0 to +1.0 in order for the ASin to work. I coded an FFloat routine that does it all at once, because it's really simple in float to just subtract a constant from the exponent.
That uses ATan2 to convert the heading to an angle, then scales it to an integer value in the range of 0 to 65535.
That's how it's done in the code I have. The GetM?? functions just get a member from the DCM matrix. M00 is row-0, column-0. M12 is row 1, column 2, and so on.
I'll try to post complete code in the next few days. I haven't been home a lot over the last few months, so I apologize for the slow progress.
Jason,
Thank you very much. Now its working But I am stiil waiting for 28 bit version.
I will test it on turning platform against centrifugal acceleration effects.
I haven't tried it on a fixed wing platform, but I suspect that a long, coordinated turn would indeed confuse it because of centrifugal force. Most likely, the "down" vector would go off a little, until the craft stopped turning and gravity returned the down vector to the correct direction. It would probably take some time for this to happen, because the acceleration vector affects the matrix very slowly.
Hello Jason! I would be very interested in seeing a snippet of how you are maintaining 28 bits of precision! I have been struggling with this for quite some time in the face of numerous multiplications/divisions. Look forward to seeing it!
The essence of it is using ** to do the multiplications instead of *. Single asterisk effectively does a 32 x 32 mult, and keeps only the lower 32 bits of the result, so you're limited to using 14 bits on each side if you want to be able to shift the results back into range when you're done:
Result := (ValA * ValB) ~> 15
If each of ValA and ValB are fixed point values with 15 bits of precision, you do the mult, and used a signed shift to get the answer back into the 15 bit range. This assumes the calculation won't overflow. If you need numbers bigger than 1.0, you'll have to use proportionally fewer bits.
Using ** does a 32 x 32 bit multiply, and returns the upper 32 bits of the 64 bit result, and handles sign properly. Using ** is basically being able to multiply by a fractional number up to, but not including, 1.0, with 32 bits of precision. Since I need to use numbers up to 3.0, and have sign, I drop four bits off each side of the inputs, so now it's basically a 4.28 x 4.28 multiply (4 'whole' bits, and 28 fractional bits, and one of the 'whole' bit. That results in an answer that has 56 bits total precision: 32 in the lower long, and 24 in the upper long. Since I want 28 bits of precision, I shift the result up four bits, so it's back to 28. If you don't need to represent numbers much above 1.0, you could use 29 or 30 bits instead of 28, and you'll keep a corresponding amount of precision when you do your multiplies.
From the prop manual:
2^29 ** 8 = 1 (8 = 2^3, 29 + 3 = 32, ie one significant bit in the result).
* and ** are the same speed in Spin - they both compute a 64 bit result, but return a different part of it, so the extra precision is "free".
I believe you can also get the roll and pitch from the DCM using atan2, this would allow you to get the Euler angles without having to do the asin in floating point, possibly save a few cycles. The CORID routine should give good results.
Hello all,
I am just bumping this thread to help people with a new interest in IMUs and DIY Drones to be able to locate it. There is a wealth of info here that can save one a lot of time.
Sorry to revive this topic. I wanted first to say thanks, I'm using the code so I can try to make a simple flight controller to the quadcopter I have (so the input is the desired angle for roll/pitch instead of the rotation rate).
Anyway, do you have a more recent version, Jason? Or just the one attached in the first post? I'm having some difficult getting the yaw angle, for some reason it's always close to 0. Oh the other hand I think pitch and roll are working fine.
And thank you again, learned a lot reading the code (first time using a parallax board and spin), so if it's the last version you have, I'm already really happy.
I have a newer version, but I'm at work and it's not in front of me.
Try the following for the yaw:
'For heading, I want an actual angular value, so this returns me an int between 0 & 65535, where 0 is forward
'Shifting it up 2 to allow for more bits of precision when accumulating the desired yaw value
YawAngle := Flt.FRound( Flt.FMul( Flt.Atan2( Flt.FFloat(DCM.GetMatrixValue(6)), Flt.FFloat(DCM.GetMatrixValue(8))), constant(32768.0 / PI) ) ) & 65535
That code is using the newer DCM, so GetMatrixValue() is returning the element of the matrix at the Nth position. It's a 3x3 matrix, so entry 6 and 8 are the M[2][0] and M[2][2] entries respectively.
And don't worry about reviving the thread - it's great to see people getting use from the code.
Hey Jason, thank you very much! Sadly, it doesn't work as expected. For some reason the behavior is similar, it starts from close to 0 and then it keeps going up slowly (but now it changes a lot more because it's an integer), and this behavior doesn't change if a rotate the board. Now, if I change the pitch (or roll, I'm not really sure, I'm using Asin of dcm[5] to get the value), the yaw changes too (and it's the same value as the pitch, but it's always positive while the pitch returns negative and positive values). Not sure why, I'm probably doing something very wrong. Maybe it's because I messed a lot with the signs of the gyroscope/accelerometer to make everything work and now my axis are wrong somehow compared to yours.
If I change Atan2 to be (Dcm8, Dcm6) instead of (Dcm6, Dcm8), then the angle get stable at 90 degrees instead of 0. Does that means something? I think I need to understand this matrix a little better before I can change many things.
I'm not sure if I get it right, but the yaw in this case is rotation around the y axis, right? Studying your code I reached the conclusion that what normally is considered the z axis is the y one in yours (because of the way you received the accelerometer, and because you assign the x/y/z gyroscope values to different axis), and that's why you calculate both the roll and pitch by getting the Asin of 10 and 12 (projection of the quadcopter Y in the Z and X axis of the earth, is it something like that?). Then, as I understand, the positions 20 and 22 of the matrix are the projection of the z axis of the quadcopter on both the x and z axes of the earth, or something like this, am I right? Sorry if I misunderstood it completely.
I generally work with a computer graphics rendering style matrix, which is +X to the right, +Y up, and +Z forward. Many accelerometer and gyro combinations treat +Y as forward and +Z as either up or down, so yes, I switch them because I'm used to the graphics style. The matrix coefficients change accordingly. If you swap the Y & Z accel & gyro values you may also need to adjust the sign of the accelerometer Z. I think this is further complicated by the fact that my original board had the gyro rotated 90 degrees.
If you can, use the DCMVisualizer program to draw a cube using the generated matrix, or just the 3 vectors for the axis values, it may help you see where you're going wrong. The code is quite sensitive to correct input. If I had to guess, I'd say that your accelerometer signs or axis assignments don't match your gyro ones - that will cause that slow drift and settling you mention.
Start without the accelerometer correction by setting CorrectionScaled[] to all zeros, and see how the gyro only inputs respond. They'll probably track your board well for a little while, but will eventually drift off. By using the matrix visualizer app on the PC you'll be able to verify if the matrix rotates along the correct axis and in the correct direction when you rotate the board. If it doesn't, swap or negate gyro values until they're correct, then work on the accelerometer values.
There are 6 possible arrangements of gyro inputs (XYZ, XZY, ZXY, ZYX, YXZ, YZX) and 8 possible arrangements of the signs, making 48 combinations possible for the inputs. You really don't want to use trial and error. :-)
Comments
Regarding the MPU-6050 chips what package are you interfacing with, MPU-6050 Sensor Board for Atmel AVR UC3 ($54.00) or the MPU-6050EVB ($69.95). I'm thinking the $ might ring a bell:) Or did you just get the chip?
I finally got my sensor stick from sparkfun 9DOF (ITG3200, ADXL345 and HMC5883) and managed to run your DCM code. After a few axis swap it works really well with your PC visualiser.
How can I get Euler Angles from the Matrix. Any help would be great.
As I see, yaw angle is pretty stable with only gyro/accel, but any chance to integrate magnetometer ?
I was able to get my adxl345 working but i've been having issues with the output of the itg-3200 and the rotation matrix. Could someone take a screen shot of the correct output so that I can have a basis to go off on? thanks
Eser- getting angles from the matrix requires doing an asin() on two of the components of the matrix - one for pitch, the other for roll, and heading is computed by doing an atan() of two more of the components. I can't remember which two, so I'll have to get back to you.
It is also possible (actually not very hard) to add a compass into the mix. I have a compass that I'm going to try it with.
The relevant wikipedia page would be this I think: http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
I have tried to convert DCM to Euler Angles. I have used below equations and fixed point math trigonometric functions (http://obex.parallax.com/objects/501/)
but It was too slow. Even running in a separate jog.
pitch = - asin(Ref[6]);
roll = atan2(Ref[7], Ref[8]);
yaw = atan2(Ref[3], Ref[0]);
How can I convert these fixed point values to float ... Then I may use F32 float library functions
Aydın
Pitch := fm.asin(fm.ffloat(Ref[6])) Pitch := fm.fneg(Pitch) Roll := fm.atan2(fm.ffloat(Ref[7]), fm.ffloat(Ref[8])) Yaw := fm.atan2(fm.ffloat(Ref[3]), fm.ffloat(Ref[0])) Dbg.tx(1) Dbg.str(fs.floattostring(Pitch)) Dbg.str(string(" ",13)) Dbg.str(fs.floattostring(Roll)) Dbg.str(string(" ",13)) Dbg.str(fs.floattostring(Yaw)) Dbg.str(string(" ",13))I think Jason puts a little magic in his math formulas, guess I will have to wait and see how he does it.DCM Matrix is in Fixed-point format 15bit whole part 16 bit fraction part 1 bit sign, because of that we cant use FFloat() function. It converts from Integer to float.
I have derived conversion to Euler formulas above from Ardupilot project.
DCM update rate is 200Hz in Jason's code. This is 5 milisec. (400.000 clockcycle) DCM algorithm is using approximately half of it.
Then we have approx 200.000 clocksycle or 2.5 milisec for conversion. Or conversion can be done in separate cog.
We need a fast way to convert fixed-point to float.
Aydin
To do the conversion, the simplest way is to convert the full fixed point number to float, then divide by (float)(1 << FixedBits), whatever that ends up being.
There's also a much faster way to do it. Floating point numbers are represented by a sign bit, an exponent, and a mantissa, and work essentially like this:
(sign) x (mantissa) x 2 to the power of (exponent)
The sign is a single bit, the mantissa is 23 bits, and the exponent is 8 bits. If you need to multiply or divide any floating point number by a power of two, you can simply add or subtract the power to/from the exponent. It's the equivalent of shifting a regular integer. So, to divide a non-zero float by (1 << FixedBits), just use plain integer math to subtract (FixedBits) from the exponent. The only slightly tricky bit is that you have to make sure it isn't zero first or you'll mess it up. So, it looks like this:
myFloat := f32.ffloat( FixedPointValue ) if( myFloat <> 0 ) 'check to make sure the float is not zero (zero looks the same in int or float) myFloat -= FixedBits 'divide the float value by (1 << FixedBits)I went a step further and implemented a function that does this in my floating point cog, so it takes an extra two instructions instead of an extra few hundred cycles.
Thank you very much, info and tips. I will try it shortly
These are the lines I use in my current version (I've altered my version of the code to use 28 fixed bits instead of 14)
rx := Flt.ASin( Flt.FFloatDiv28( DCM.GetM12 ) ) 'Convert to float and divide by (float)(1<<28), then take ArcSine rz := Flt.ASin( Flt.FFloatDiv28( DCM.GetM10 ) ) 'Convert to float and divide by (float)(1<<28), then take ArcSine ry := Flt.Atan2( Flt.FFloat(DCM.GetM20), Flt.FFloat(DCM.GetM22) )You don't need to scale down if you're using Atan2 because it doesn't care what the range of inputs is - it's just converting from a coordinate pair to an angle. ArcSin requires the inputs be in the range of -1 to 1, so I scale them down by FixedBits before doing those conversion.
The results are independent of each other - rx = amount of pitch around the X axis, ry = amount of roll around the Y axis, and rz = heading. This code does not handle what happens when the craft is inverted.
Jason
I am trying the code below, but I have no meaningfull results
Pitch := 0.0 repeat Gx := Gyro.GetRX Gy := Gyro.GetRY Gz := Gyro.GetRZ Ax := Gyro.GetAX Ay := Gyro.GetAY Az := Gyro.GetAZ 'Magic happens here UpdateLoop Tx.Str(string(16, 1, 10, 13)) ' I have tried both ways 'Tmp1 := F.FFloat(Ref[6]) 'if (Tmp1 <> 0) ' Tmp1 -= FixedBits Tmp1 := F.FDiv(F.FFloat(Ref[6]),16384.0) Pitch := F.FNeg(F.Degrees(F.ASin(Tmp1))) Tmp2 := F.FRound(F.FMul(Pitch,1000.0)) 'convert to integer to send serial Tx.str(string("Pitch = ")) Tx.Dec(Tmp2) LoopTime := cnt - LastTime waitcnt( constant(80_000_000 / 200) + LastTime ) LastTime += constant(80_000_000 / 200)many thanks for the Video and the DCM-Visualizer..... :-))
i have this done on Linux and opengl , but it was the wrong way .... i have working with a 3D-Quadcopter model, so it was too slow.
your video gives me a new way thanks
in the last weeks i work on a new QuadCopter Body with C3 (LaserRangeFinder, GPS, XBEE, and intercommunications to my PPB as master)
and the PPB ( L3G4200D (3AxisGyroscopte) PID , 6channel-RC-Receiver and 4-channel ServoPort-ESC)
I find your DCM - code verry cool
thanks for sharing
regards nomad
Do you have any progress on your DCM code. Did you ever try it on a fixed-wing platform. I mean effects of centripetal acceleration.
Did you have a version of DCM code with euler angles output? Because I have tried to convert euler angles from DCM, but I failed.
Thanks
I'll have to clean it up a bit before I can post the whole thing, but essentially the DCM to angles conversion is this:
Roll := Flt.ASin( Flt.FFloatDiv28( DCM.GetM12 ) ) 'Convert to float, then divide by (float)(1<<28)
Pitch := Flt.ASin( Flt.FFloatDiv28( DCM.GetM10 ) ) 'Convert to float, then divide by (float)(1<<28)
The new version of the DCM code maintains 28 bits of integer precision, so AFTER converting to float, I have to scale the numbers down by 1<<28 to bring them into the range of -1.0 to +1.0 in order for the ASin to work. I coded an FFloat routine that does it all at once, because it's really simple in float to just subtract a constant from the exponent.
Heading is a little different:
Heading := Flt.FRound( Flt.FMul( Flt.Atan2( Flt.FFloat(DCM.GetM20), Flt.FFloat(DCM.GetM22)), constant(32768.0 / PI) ) ) & 65535
That uses ATan2 to convert the heading to an angle, then scales it to an integer value in the range of 0 to 65535.
That's how it's done in the code I have. The GetM?? functions just get a member from the DCM matrix. M00 is row-0, column-0. M12 is row 1, column 2, and so on.
I'll try to post complete code in the next few days. I haven't been home a lot over the last few months, so I apologize for the slow progress.
Thank you very much. Now its working
I will test it on turning platform against centrifugal acceleration effects.
Result := (ValA * ValB) ~> 15
If each of ValA and ValB are fixed point values with 15 bits of precision, you do the mult, and used a signed shift to get the answer back into the 15 bit range. This assumes the calculation won't overflow. If you need numbers bigger than 1.0, you'll have to use proportionally fewer bits.
Using ** does a 32 x 32 bit multiply, and returns the upper 32 bits of the 64 bit result, and handles sign properly. Using ** is basically being able to multiply by a fractional number up to, but not including, 1.0, with 32 bits of precision. Since I need to use numbers up to 3.0, and have sign, I drop four bits off each side of the inputs, so now it's basically a 4.28 x 4.28 multiply (4 'whole' bits, and 28 fractional bits, and one of the 'whole' bit. That results in an answer that has 56 bits total precision: 32 in the lower long, and 24 in the upper long. Since I want 28 bits of precision, I shift the result up four bits, so it's back to 28. If you don't need to represent numbers much above 1.0, you could use 29 or 30 bits instead of 28, and you'll keep a corresponding amount of precision when you do your multiplies.
From the prop manual:
2^29 ** 8 = 1 (8 = 2^3, 29 + 3 = 32, ie one significant bit in the result).
* and ** are the same speed in Spin - they both compute a 64 bit result, but return a different part of it, so the extra precision is "free".
Roll = atan2(dcm[8], dcm[5])
Pitch = atan2(dcm[8], -dcm[2])
Yaw =atan2(dcm[0], dcm[1])
*I havent tested it yet*
I am just bumping this thread to help people with a new interest in IMUs and DIY Drones to be able to locate it. There is a wealth of info here that can save one a lot of time.
So now, all this is very much need-to-know info. This thread should really have a Sticky somewhere for guys into DIY Drone development.
Anyway, do you have a more recent version, Jason? Or just the one attached in the first post? I'm having some difficult getting the yaw angle, for some reason it's always close to 0. Oh the other hand I think pitch and roll are working fine.
And thank you again, learned a lot reading the code (first time using a parallax board and spin), so if it's the last version you have, I'm already really happy.
Try the following for the yaw:
That code is using the newer DCM, so GetMatrixValue() is returning the element of the matrix at the Nth position. It's a 3x3 matrix, so entry 6 and 8 are the M[2][0] and M[2][2] entries respectively.
And don't worry about reviving the thread - it's great to see people getting use from the code.
If I change Atan2 to be (Dcm8, Dcm6) instead of (Dcm6, Dcm8), then the angle get stable at 90 degrees instead of 0. Does that means something? I think I need to understand this matrix a little better before I can change many things.
I'm not sure if I get it right, but the yaw in this case is rotation around the y axis, right? Studying your code I reached the conclusion that what normally is considered the z axis is the y one in yours (because of the way you received the accelerometer, and because you assign the x/y/z gyroscope values to different axis), and that's why you calculate both the roll and pitch by getting the Asin of 10 and 12 (projection of the quadcopter Y in the Z and X axis of the earth, is it something like that?). Then, as I understand, the positions 20 and 22 of the matrix are the projection of the z axis of the quadcopter on both the x and z axes of the earth, or something like this, am I right? Sorry if I misunderstood it completely.
If you can, use the DCMVisualizer program to draw a cube using the generated matrix, or just the 3 vectors for the axis values, it may help you see where you're going wrong. The code is quite sensitive to correct input. If I had to guess, I'd say that your accelerometer signs or axis assignments don't match your gyro ones - that will cause that slow drift and settling you mention.
Start without the accelerometer correction by setting CorrectionScaled[] to all zeros, and see how the gyro only inputs respond. They'll probably track your board well for a little while, but will eventually drift off. By using the matrix visualizer app on the PC you'll be able to verify if the matrix rotates along the correct axis and in the correct direction when you rotate the board. If it doesn't, swap or negate gyro values until they're correct, then work on the accelerometer values.
There are 6 possible arrangements of gyro inputs (XYZ, XZY, ZXY, ZYX, YXZ, YZX) and 8 possible arrangements of the signs, making 48 combinations possible for the inputs. You really don't want to use trial and error. :-)