DCM (step by step) Hopefully!!!!

Shawna

I have been working on a quadcopter for I do not know how many months. I have been using a simple complementary filter for fusion of my Accelerometer and Gyroscope. While my results were actually better than expected, I want to expand on how well my complementary filter handles this fusion of the sensors.

I am going to attempt a Direct Cosine Matrix(DCM) approach. I have been doing a lot of reading on the topic so far but to be honest I am still lost.

My first assumption is that the results of the Matrix are still fussed together using a complementary filter, in one of the articles I have looked at I am pretty sure I saw the comp. filter at the end of there loop.

I do not have any real questions yet, but I wanted to get a thread started for when I do.

Here is two links that I have been reading to try and understand the DCM, is there any easier tutorials to read through.

This link I believe is geared towards fixed winged air craft and I do not understand why.
https://code.google.com/p/gentlenav/downloads/detail?name=DCMDraft2.pdf&



This link I have been spending a lot of time reading but I am having a hard time following it.
http://www.starlino.com/dcm_tutorial.html

There was one more but now I can't find it, I need to dig some more.

I do not know if I will be able to figure this out and implement it. This could be a long process, just trying to understand the complementary filter took forever, but with everyone's help and patients I did it. Thanks

Shawn

Shawna

Reserved for something. Hopefully links.

Shawna

Reserved for something else. Hopefully steps to the DCM.

Shawna

This is the other link that I was thinking of. I am not sure if it is talking about DCM or not. It is very close if it is not, I am still reading through the info. Isn't the internet great, so much info and so little time to read it.

http://www.starlino.com/imu_guide.html

JasonDorie

The Starlino DCM stuff is really dense. At first it looks like he's saying, "this is easy, and with really basic math skills you can get it!" and then he just dives into dense algebra. I hate that. Read the DCM Draft paper by Premerlani. Then read it again. Then read it again... I read through it probably 15 times before I got it all, so don't be discouraged if you don't understand it right away.

Their paper uses a fixed wing aircraft because that's what they developed it for - An autopilot for a plane. They make the assumption that their "compass" will actually be derived from GPS locations since a plane can only move forward. Aside from that, everything is the same whether it's a quad or a plane.

A DCM (direction cosine matrix) is just a rotation matrix, also called a 3x3 orthonormalized (orthogonal normalized) matrix. Orthogonal means that the vectors that make up the rows & columns of the matrix are perpendicular to each other, and normalized means they all have a length of 1.0.

The DCM IMU code starts with a unit matrix and just updates the orientation of that matrix by rotating it a little bit every time the system goes through a loop. If you want to visualize what it's doing, try this:

Find a small box, like a shoe box, and put it on the table in front of you. Draw 1 inch long lines along the edges of the box touched by that corner, starting at the corner. You'll have one line going forward along the bottom left edge, one line going right along the bottom back edge, and one line going up along the back left edge. These three lines are your matrix vectors. Now, every time through your update loop, read the gyro to get your angular speeds and multiply those numbers by how much time has passed since your last reading (1/200th sec). Those numbers are now "how far did we rotate since last time". Take those angles and rotate your box by that much for each axis.

Every time you do this, the vectors you drew on the box will be pointing in slightly different directions. The coordinates of these vectors, relative to the corner of the box you drew them from, are what the DCM code is tracking. That's it. Everything else is just details, like keeping the vectors perpendicular to each other and unit length (re - ortho-normalizing), and blending in the accelerometer contribution over time.

The vectors need to be re-normalized (make sure the length is 1.0) and made perpendicular again because rotating them around with fixed point math introduces small errors. Over time these would accumulate. If the vectors in the matrix shrink or lengthen, or they aren't properly at 90 degrees to each other, then the math based on them that extracts angles or maintains them won't work as well.

Hopefully that helps you visualize what's going on. It's a simple thing that it's doing, but it takes some non-simple math to get it done.

Shawna

Ok, first off I am going to attempt this with floating point because scaling integers hurts my brain. I realize that floating point is slow but I need real numbers to work with.

Before I can get to far into this I need help understanding a few things.

1) Starting with a unity matrix which has 1's diagonally from upper left to lower right, and the rest 0's. How does that relate to radians or degrees of rotation? Also reading from my sensors are always greater than 1. Now if I normalize the ACC readings by doing this => gravity sqrt = (aX^2) + (aZ^2) + (aZ^2) and the ACC is not moving the answer should be 1. I realize that the matrix is rotated but it looks like the matrix is always normalized at the end of the loop. 2PI would be 360 degrees 1 would only be 57 degrees approximately. I am missing some fundamental math somewhere. It has been so long since I have done any real Trig, or math for that matters.

2) Back to the Integer, Float battle in my brain. Jason in your DCM code you do this. Fixed_One = 1 << FixedBits, where FixedBits = 15. Is this how you deal with values less than 1 using integers? So then if I were to display Fixed_One as a decimal it would actually display 32768, is that correct?

3) I thought that a vector was just the length of a line from its origin. But in the DCM paper they show a vector like this.

[ Qx]
Q = [ Qy]
[ Qz]

I realize I have not read the paper enough, but I am missing some very simple things that I can't seem to understand even by looking them up on good old wiki. I understand it better now than I did and I feel like if I could just work out a few technical things it would start to click.

4) I would assume that both the ACC and the Gyro have to be converted into the same units in order to mix them. I think that would have to be degrees or radians. With just a simple Comp filter I used degrees.

5) Is the world rotating around the quad or is the quad rotating around the world?

I am struggling and need some direction.

I understand the shoe box explanation above Jason Thanks.

Shawn

Mark_T

You need to find a good introduction to vectors and transformations - start with 2D then move to 3D. Once you've grokked
that will be the time to considered DCM algorithms. You need to understand the difference between a vector and its representation
relative to some coordinate frame. You need to understand dot products and perhaps cross products too.
DCM is just a 3D transformation matrix that happens to have only rotation.

All the time in IMU calculations you have to decide which frame of reference you are talking about, global or local, and a
DCM represents the mapping from one to the other (and you have to know which way it maps).

JasonDorie

Shawna wrote: »

1) Starting with a unity matrix which has 1's diagonally from upper left to lower right, and the rest 0's. How does that relate to radians or degrees of rotation? Also reading from my sensors are always greater than 1. Now if I normalize the ACC readings by doing this => gravity sqrt = (aX^2) + (aZ^2) + (aZ^2) and the ACC is not moving the answer should be 1. I realize that the matrix is rotated but it looks like the matrix is always normalized at the end of the loop. 2PI would be 360 degrees 1 would only be 57 degrees approximately. I am missing some fundamental math somewhere. It has been so long since I have done any real Trig, or math for that matters.

It sort of doesn't, and sort of does. The columns of a matrix are vectors, and those vectors point in the direction of the axis they represent. An identity matrix [1,0,0] [0,1,0] [0,0,1] has the vector [1,0,0] as it's first column. That vector represents the direction of the X axis in the local space of the matrix. In this case, that vector points toward 1 X, 0 Y, and 0 Z, which means it points along the X axis of the world. If you rotate the matrix, the numbers in the first column will probably point somewhere else. This means that the LOCAL x-axis of the matrix (or the object whose rotation you're tracking) is now pointing in the direction of this vector. It's not angles, or radians, it's just pointing in a direction. To get angles from it, you have to use trigonometry. Mark_T's suggestion of starting in 2D and working up to 3D is a good one, because the relationships are a little simpler in 2D.

Here's a start: Sin(0) is 0, and Cos(0) is 1. Sin and Cos of an angle are used to produce the X and Y values of a vector that points in the direction of that angle. (look at this : http://en.wikipedia.org/wiki/File:Sine_and_Cosine_fundamental_relationship_to_Circle_(and_Helix).gif )

Shawna wrote: »

2) Back to the Integer, Float battle in my brain. Jason in your DCM code you do this. Fixed_One = 1 << FixedBits, where FixedBits = 15. Is this how you deal with values less than 1 using integers? So then if I were to display Fixed_One as a decimal it would actually display 32768, is that correct?

Yes, exactly right. Basically you're scaling everything up by (1 << 15) then scaling it back down if/when necessary. For example, 1.5 x 1.5 = 2.25. If you scaled everything up by 10, it'd be 15 x 15 = 225. You then have to divide the result by (10 x 10) to get your 2.25 back. It works no matter what you scale up by, as long as you scale up enough to keep the precision you need. In my same example, I could have scaled up by 2 instead:

1.5 * 1.5 scaled up by 2 becomes 3 x 3 = 9.
9 / (2 x 2) = 2.25. (You scale down the result by double what you scaled up by when multiplying. You scale up first in order to do division, adding and subtracting do not require scaling).

Shawna wrote: »

3) I thought that a vector was just the length of a line from its origin. But in the DCM paper they show a vector like this.

[ Qx]
Q = [ Qy]
[ Qz]

I realize I have not read the paper enough, but I am missing some very simple things that I can't seem to understand even by looking them up on good old wiki. I understand it better now than I did and I feel like if I could just work out a few technical things it would start to click.

A vector is the coordinates of a line from its origin. In your example above, Qx, Qy, and Qz represent magnitudes of the X, Y, and Z components of the vector. So if they were Qx = 0, Qy = 1, and Qz = 0, the vector would be perfectly vertical (assuming you decide that +Y is up).

Shawna wrote: »

4) I would assume that both the ACC and the Gyro have to be converted into the same units in order to mix them. I think that would have to be degrees or radians. With just a simple Comp filter I used degrees.

Not entirely true. You do have to know what units and scales you're using, but a matrix doesn't use radians, it's just vectors. The gyro is actually the one that needs more scaling, because you need to convert the gyro readings into radians so you can rotate the matrix by them. Adding in the accelerometer contribution just uses vector math, not rotations, and the accelerometer value is already a vector.

Shawna wrote: »

5) Is the world rotating around the quad or is the quad rotating around the world?

Yes. The matrix being computed has the property that it can be used to orient something from world to local space, or from local to world space. The columns of the matrix represent the local axis' of the quad in the world. On the other hand, the ROWS of the same matrix represent the x, y, and z axis' of the world, in local space (the space of the quad). In my DCM code, I use this to compute where the world is relative to the quad, instead of where the quad is relative to the world, because it's a little easier to work with that way.

Mark_T is right in that the DCM tracks one or the other, but one of the properties of an orthonormal rotation matrix is that it's transpose (flipping from rows to columns) is its inverse (the opposite). So technically it's tracking both things at the same time: how to get from quad space to world space, and how to get from world space to quad space.

Shawna

Ok, you guys be the judge. Tell me if I am learning anything or if I am still running in the wrong direction.

I have read a bunch on matrices, vectors, and transformations. It is still pretty confusing but every time I read through a new tutorial a little more sinks in.

1) If I am putting this together right the unity matrix is basically the starting point for determining attitude. With all three axis at 1's diagonally, from top left to lower right, the quad would be perfectly level facing north. With the unity matrix all three axis are perpendicular which is why cosine works to give us angles, it forms right angle triangles.

2)The next part is speculation on my part because I do not completely understand the rotation matrix yet. As the earth moves with respect to the body of the quad, or vise versa, how ever you want to do it. I would convert gyro readings into radians, by doing this I now have the magnitude of the unity vector which is 1, and the radians of how far the earth has rotated about the body. By knowing these two things I should be able to tell the magnitude of the vector that I have moved. Which I think can be compared to my acc reading which is all ready a vector. This would give me my error.

3) I now have to make sure that my new matrix which has been rotated is still a unity matrix by normalizing it, if it is not, then my axis will no longer be perpendicular to each other which will also give me errors in my calculation.

Am I close, or am I going completely in the wrong direction?
I still do not understand where the values from my ACC and Gyro are plugged into the matrices.
I looked at Jason's DCM demo but I did not understand where he was using cosine or sine in his equations.
I still need to read more on transpose and dot matrices also.
This is another link I found, it is kind of vague but at the same time its not real dense with math since it starts with a 2D matrix.

http://mathworld.wolfram.com/RotationMatrix.html

Thanks
Shawn

JasonDorie

You're closer, but still not quite there. I would recommend reading about 2D matrices first, because that'll help when you transition to 3D matrices.

A rotation matrix can be rotated by another rotation matrix. The resulting new matrix is the "end result" orientation you'd get if you rotated something by the first matrix, then the second one. They are "order dependent" meaning that the order in which the rotations are applied is important. (at least, in 3D they are. In 2D you can only really rotate around one axis).

If you take an angle, plug that value into sin & cos, you'll get out some numbers. Those numbers are the lengths of the axis of a 2D vector that was rotated by that much. So, for example, in 2D, a rotation matrix has this form:

[ 11 , 12 ]
[ 21 , 22 ]

(The numbers are the values of the row and column, so 11 = row 1 , column 1). A unit matrix in 2D looks like this:

[ 1 0 ]
[ 0 1 ]

The first vertical column is [1, 0]. Those are the X & Y components of a unit vector that points along the X axis (X = 1.0, Y = 0.0). The second column is a unit vector pointing along the Y axis (X=0, Y=1).

A 2D matrix for any 2D rotation looks like this:

[ Cos , -Sin ]
[ Sin , Cos ]

If you take a 2D rotation matrix and multiply it by another 2D rotation matrix, the result will be a 2D rotation matrix that is the first rotation followed by the second rotation. This holds true for 3D rotation matrices.

"Multiply" in this case isn't an entirely simple operation - Multiplying two matrices together isn't just multiplying the components, it's actually multiplying and adding rows and columns together in a specific way. Have a look at http://en.wikipedia.org/wiki/Matrix_multiplication

It might be confusing, but read it a few times and work through it. That's what one of the functions in the DCM code is doing - it takes the current orientation matrix and rotates it by a matrix that I make from the gyro readings.

Note that I never actually make sin/cos values of the gyro though, because the gyro angles are so small. If an angle is really small, the sin of the angle is basically the same as the angle in radians, and the cos is close to (1 - angle) in radians. This is an approximation, but it works. It would be "more correct" to actually take the sin/cos values of the gyro angles, multiplied by the time delta (time step value between updates). The approximation is much faster than actually taking the sin/cos, and for really small angles it works.

Shawna

Ok, I am going to start putting steps together here.

1) Set up a unity matrix in 3 x 3 form. Which is a orthogonal normalized matrix, orthogonal meaning perpendicular and normalized meaning vector lengths of 1. It would look like this.

[ 1,0,0 ]
[ 0,1,0 ]
[ 0,0,1 ]

2) Read my gyro values and take them times my dT, like this. , ,

gX = gX * dT gX is my pitch axis for my gyro.
gZ = gZ * dT gZ is my yaw axis for my gyro.
gY = gY * dT gY is my roll axis for my gyro.

3) Now I will put the gyro values into another 3 x 3 matrix.

[ 1, -gZ,gY ]
[ gZ, 1,-gX ]
[ -gY,gX, 1 ]

4) Then I will multiply the identity matrix in step 1 with the matrix in step 3.

Rotation Matrix =

[ 1,0,0 ]
[ 0,1,0 ]
[ 0,0,1 ]

X

[ 1, -gZ,gY ]
[ gZ, 1,-gX ]
[ -gY,gX, 1 ]

I am not sure how to type these equations in the post, I tried to wrap it in code tags with no success.


Questions
1) I still do not understand what order my pitch,roll, and yaw axis should be place in the matrix in step 3?
2) Is there a special name for the matrix in step 3? (Edit 7-25-13 I am not sure what this matrix is I had thought it was an antisymmetric matrix, but I am incorrect.)
3) In step 4 I am not sure which matrix should be first in the formula, the unity or the antisymmetric matrix.
4) Is the product of step 4 a dot product? I understand how to multiply to matrices together.

How is this looking so far?


Shawn

Shawna

I think I just learned something new. This is my current mind set on trying to understand this.
If a 3 axis gyro was manufactured perfectly meaning that all 3 axis are orthogonal (perpendicular) to each other ( which they probably are I have not look at the tolerances on the data sheet), then no matter what type of rotations occur to it in a given time frame, the degrees of rotation should still be orthogonal for all 3 axis's when graphed, or plotted. If they are not then the axis's need re-normalized. By re-normalizing the matrix I think I can now find the error from the gyro readings. I think this thought process is right.

If the above is close to right, is this how the matrix makes up for the yaw during roll or pitch errors?

Shawn

Shawna

Jason, you said this to me a couple of posts ago.

The gyro is actually the one that needs more scaling, because you need to convert the gyro readings into radians so you can rotate the matrix by them.

I have been reading through your DCM code trying to follow the steps in William Premerlani and Paul Bizard DCM paper. This is the only thing I can see that you are doing to the gyro values.

'Convert the gyro readings into proper range for the DCM inputs.  Also, swap the axis
  'around and adjust signs to coincide with my frame of reference
  iGz := -Gx / 5
  iGx :=  Gy / 5
  iGy := -Gz / 5

I do not understand the significance of dividing gX by 5. The raw readings from the ITG3200 gyro are in degrees per second. In order to convert the gyro readings into radians I would think you would have to do this. gXradians = (gX / 14.375 * dT) / 57.32

Is dividing gX by 5 just filtering the gyro values, and then using the filtered values as the magnitude for the 3 vectors of the body?

Thanks
Shawn

JasonDorie

I'm on vacation for a week or two, so my responses are going to be spotty, and through mobile, so sorry in advance if they're kind of sparse.

The / 5 is what I ended up with after measuring and simplifying. It's a conversion factor between gyro values and the input required for the DCM to produce readings that are stable.

It's close to this:

1 / 14.75 degrees per sec = 1 gyro bit
PI / 180 = degrees to radians
everything / 200 updates per second
everything * (1<<14) = fixed point

After working through all of it, it ends up simplifying down really close to "divide by 5", so that's what I used. The paper says to do it empirically. IE - put the unit on a turntable and compare the real orientation with the estimated one, and adjust your scale factors until the two match. This is likely because of the inputs to the anti-symmetric matrix being "approximate radians".

Shawna

Ok, I was wrong in post 11 step 3. The matrix there is not an antisymmetric matrix. An antisymmetric matrix would require 0 running diagonal from upper left to lower right.

One step back.

Shawna

In post 11, question 3, I asked this.

3) In step 4 I am not sure which matrix should be first in the formula, the unity or the antisymmetric matrix.

The order of the multiplications are important. According to the DCM draft paper, the new data from the gyros always goes on the left side of the equation.

Shawna

So in post #11 step #4, multiplying the 2 matrices together I come up with this spin code.

'This is what I came up with when I multiply the unity matrix and the matrix with all of my gyro readings.
  'For some reason my gX,gZ and gY signs are all opposite of Jason's.
  'Column 1
  TempMat[0]   := ( One * RotMat[0] + -gZ  * RotMat[1] +  gY  * RotMat[2]) ~> 15
  TempMat[1]   := ( gZ  * RotMat[0] +  One * RotMat[1] + -gX  * RotMat[2]) ~> 15
  TempMat[2]   := (-gY  * RotMat[0] +  gX  * RotMat[1] +  One * RotMat[2]) ~> 15
  'Column 2
  TempMat[3]   := ( One * RotMat[3] + -gZ  * RotMat[4] +  gY  * RotMat[5]) ~> 15
  TempMat[4]   := ( gZ  * RotMat[3] +  One * RotMat[4] + -gX  * rotMat[5]) ~> 15
  TempMat[5]   := (-gY  * RotMat[3] +  gX  * RotMat[4] +  One * RotMat[5]) ~> 15
  'Column 3
  TempMat[6]   := ( One * RotMat[6] + -gZ  * RotMat[7] +  gY  * RotMat[8]) ~> 15
  TempMat[7]   := ( gZ  * RotMat[6] +  One * RotMat[7] + -gX  * RotMat[8]) ~> 15
  TempMat[8]   := (-gY  * RotMat[6] +  gX  * RotMat[7] +  One * RotMat[8]) ~> 15  

This code I pulled from Jasons DCM Demo Program

    long[Dest][0] := (long[Mat][0] *  F1 + long[Mat][1] *  Rz + long[Mat][2] * -Ry) ~> FixedBits
    long[Dest][1] := (long[Mat][0] * -Rz + long[Mat][1] *  F1 + long[Mat][2] *  Rx) ~> FixedBits
    long[Dest][2] := (long[Mat][0] *  Ry + long[Mat][1] * -Rx + long[Mat][2] *  F1) ~> FixedBits
     
    long[Dest][3] := (long[Mat][3] *  F1 + long[Mat][4] *  Rz + long[Mat][5] * -Ry) ~> FixedBits
    long[Dest][4] := (long[Mat][3] * -Rz + long[Mat][4] *  F1 + long[Mat][5] *  Rx) ~> FixedBits
    long[Dest][5] := (long[Mat][3] *  Ry + long[Mat][4] * -Rx + long[Mat][5] *  F1) ~> FixedBits
     
    long[Dest][6] := (long[Mat][6] *  F1 + long[Mat][7] *  Rz + long[Mat][8] * -Ry) ~> FixedBits
    long[Dest][7] := (long[Mat][6] * -Rz + long[Mat][7] *  F1 + long[Mat][8] *  Rx) ~> FixedBits
    long[Dest][8] := (long[Mat][6] *  Ry + long[Mat][7] * -Rx + long[Mat][8] *  F1) ~> FixedBits

For some reason my gX,gZ and gY signs are all opposite of Jason's. I have not figured that part out yet but I am getting closer.

Shawn

JasonDorie

The difference would be keeping track of the world in quad space, or keeping track of the quad in world space. Either way it'll work out the same - the rows and columns might be flipped, but that should be it.

prof_braino

Just found this thread. You guys are great.

Shawna

I have been reading William Premerlani and Paul Bizard's DCM paper and running through Jason's 15 Bit DCM code to try and put it all together. While playing with Jason's code last night I was finally able to get it to function as he showed on YouTube. I was so excited by how it tracked my proto board, I had to try and implement it in my flight code. So I spent the last few hours replacing my simple comp filter with Jason's DCM code. I just flew it outside and it tracked orientation perfectly.

DCM is by far superior to the simple complimentary filter.

Jason, your the man. Thank You for putting this together and sharing it with us.

I am by no means done with this thread, I still want to understand this enough to write my own DCM and be able to add different things to it, such as a magnetometer and or GPS, or whatever I can imagine. Once I get the DCM figure out I think the big challenge will be to figure out how to speed the program up. Hurry up Parallax and get the Prop2 out.

Thanks Again

Shawn

Shawna

Here is a video of the quad flying with Jason's 15 bit DCM. The video is shaky but my wife didn't charge much to shoot it. Beggars can't be choosers I guess.

Shawna

Some how I overlooked this during my reading, not necessarily overlooked but it failed to register with me. Once the matrix is rotated the first time, all the necessary components are in the matrix to solve for either the world or the quad orientation. Is that right?
So then, the rows would be for the quad orientation and the columns would be for the world orientation, right, at least in your code Jason? So in my code with the reversed signs my world and quad orientations would be flopped compared to yours. I understand that if my logic is correct.

I had a hell of a time extracting angles for pitch and roll out of your DCM code(because of my lack of understanding). From the looks of your DCM thread those other guys must of got there hands on your 28 bit version. Jason, did you see a real difference in stability between the 15 bit and 28 bit version of your code?


Thanks
Shawn

Duane Degn

Shawna wrote: »

Here is a video of the quad flying with Jason's 15 bit DCM. The video is shaky but my wife didn't charge much to shoot it. Beggars can't be choosers I guess.

Wow! That looks great. Thanks for posting the video.

I need to get back to my flying projects. I still haven't flown a quad using Jason's code yet.

Very cool quadcopter.

JasonDorie

Shawn - fantastic work! It's exciting to see your quad working so well! My own isn't quite that stable yet, but I haven't had time to rebuild the body to isolate the board from vibration. My own is using the 28 bit version, but I don't think it makes much difference in the stability. The largest improvements have been in the update speed - I was able to bump it up to 250hz and still have time left over. Extracting the angles took me a while too.

Shawna

In the video the quad is using your 9DOF object that I modified with an 11 point median filter. Then I added an acc low pass filter to your DCM code. I also modified the section of your DCM code that offsets the gravity at startup. I put hard values in there so that the quad can be set on non-level ground and still figure out where level is. When the quad is powered up it needs to sit for about 10 seconds and then it is ready to rock.

I understand the rotation part of the DCM, and I understand that the matrix needs re-normalized but I have not been able to equate your code to the DCM Draft paper. Every time I read the paper and then look at your code I pick up one more little thing.

Jason are you using the I portion of your PID, I am not using it. I think that if I could figure out how to tune the I portion, the quad would be even more stable.

Thanks Again
Shawn

Shawna

Oh ya, if anyone else is building a quad using a DCM matrix, probably Jason's because I have yet to see another one written in spin. Be careful what values you use out of the rotation matrix to solve for pitch and roll angles. If you choose the wrong elements out of the matrix you can still calculate pitch and roll but you will still have problems with the dreaded yaw during pitch or roll problem.

I am not sure why this is yet, but I have a suspicion that it has something to do with mixing values from the world orientation versus the quad orientation.

Shawna

My brain has been stuck for awhile in a holding pattern with this DCM stuff. I have been working on other aspects of the project though. Here is a kind of related question. I have been running in to speed problems with my code because to be honest I am not that clever at coding spin yet. So I said screw it lets see what happens when I slow my flight loop way down. I cut my flight loop from 200Hz to 100hz and now I should have time to do all the stuff I want to do. In order to make Jason's DCM handle this I had to modify this section of code.

  iGz :=   Gx / 5  
  iGx :=  -Gy / 5
  iGy :=  -Gz / 5

I changed it to this.

  iGz :=  ( Gx * 10)/23  
  iGx :=  (-Gy * 10)/23
  iGy :=  (-Gz * 10)/23

Now my question is, what do you guys think I should have to do to my PID loops since I slowed the flight loop down. Should I have to increase the PID values or decrease the values since I am only running the loops at half the speed as before? I think the correct answer is increase the PID values but some input would be welcome

I am still not sure if this is going to work, but I can say this. When I cut the speed of my plain complementary filter code to 100Hz the quad was almost impossible to fly and there was ridiculous lag in the stick inputs.

When I cut the speed of Jason's DCM code the quad seems to handle pretty good and response is still excellent. The quad acts like I just need to tune the PID loops a little, and then I am not sure if I would even notice a difference between the two speeds.

JasonDorie

The PID loops will be timing sensitive - the D component will likely need to be adjusted, since it's "velocity" based, as opposed to the P component, which is just instantaneous error. I'm not sure exactly which direction they'll need to be tuned though - I haven't played with that stuff enough to know.

With the complimentary filter, you'd need to adjust the "contribution amounts" because they're tied to how fast the convergence happens between the accelerometer & gyro. In the DCM code, you'll probably want to lower the number that affects the error contribution to account for the slower update. (I think it's a / 512, so you'd change it to / 256 to get approximately the same time convergence). You might also want to lower the sample rate of the gyro & accelerometer, and change the number of samples in your median filter. (or not - the higher sample rate for those might actually work in your favor).

J

Shawna

Well after doing the wifes to do list, I had a battery worth of time to tweak on the 100Hz DCM quad code. I found suitable settings that I think will match the performance of my 200Hz flight loop. I need to get it to the park and video the flight. I kind of had to cheat though, in my 200hz flight loop I only used P and D from the PID loops. In the 100Hz flight loop I could not get the results I wanted and had to add some I to the PID. After seeing how much the I value helped with the 100hz flight loop I need to go back and add it to the 200Hz loop.

So if the 100Hz loop suceeded in making a stable and smooth quad then I will have 10mS of time to do other stuff. I should be able to add a compass with that much time.

For some reason I can not find this in your DCM code Jason.

In the DCM code, you'll probably want to lower the number that affects the error contribution to account for the slower update. (I think it's a / 512, so you'd change it to / 256 to get approximately the same time convergence).

Shawn

JasonDorie

It's this code here:

  'Scale the error value down enough to feed back into the update loop.  Should convert this to a PID (without D)
  ErrorAxis[0] ~>= 8   
  ErrorAxis[1] ~>= 8   
  ErrorAxis[2] ~>= 8   
   
  CorrectionScaled[0] := ErrorAxis[0]
  CorrectionScaled[1] := ErrorAxis[1]
  CorrectionScaled[2] := ErrorAxis[2]   

Change the ~>= 8 to ~>= 7 to account for the halving of the update rate.

J

Shawna

Oh, I remember. 2^8 = 255 Duh.
Thanks.

I just flew the quad outside with the 100Hz flight loop I think it will fly well with the proper tweaking. I will post a video when I have a chance to shoot one. Anyways I am starting to derail this thread so I will try and get back on topic.

Thanks
Shawn