Hey Jason, I have a quick question about your DCM code. If I want to shut off the ACC correction for your DCM can I just comment out this part. It seems to do the job but I was just curious if I am right. I want to see how stable it is with just the gyro.
'Gravity (or some approximation thereof) is the accelerometer
GetAccelAsVector( @Gravity ) 'Swaps the axis, scales the values
Gravity[0] -= AccelLevel[0] 'Offset the accelerometer, assuming it was level on startup (HACK)
Gravity[1] -= AccelLevel[1]
Gravity[2] -= AccelLevel[2]
'GravityNorm = Gravity.Normalized();
NormalizeVector( @Gravity )
'FixedVector WorldUp = new FixedVector(BodyEstCorr.m[1, 0], BodyEstCorr.m[1, 1], BodyEstCorr.m[1, 2]);
'ErrorAxis = GravityNorm.Cross(WorldUp)
CrossVector( @Gravity, @Ref[3], @ErrorAxis[0] )
'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]
Ok, lets see if I kind dabble a little further into this rotation matrix quest. In post 11 I rotated the rotation matrix with my new gyro values. Every time the rotation in post 11 is done there will be numerical errors introduced into the matrix that will cause it to no longer be orthogonal. The dot product of any two rows of an orthogonal matrix should be 0. Because of this error or lack of orthogonality the matrix needs to be re-normalized.
To do this I first have to calculate the error.
To find the error I will take the transpose of row 1, and take it times row 2. This should give me the error. Doing this I believe is finding the DOT product of the 2 rows.
The next step would be to give half of this error to each the X and Y row and then rotate them in the opposite direction by cross coupling.
Now row X and Y should be orthogonal again.
To make row Z orthogonal again I would simply take the cross product of rows X and Y.
The final step of the re-normalization proccess would be to scale the rows so that they have a magnitude of 1.
And now I just confused myself again. I think I stripped the right therory out of the Premerlani and Bizard paper to this point, but I need to refresh myself with the dot and cross products again. And then figure out how to convert the math to spin.
I think I am going to have some time to play with the quad this weekend, as long as it isn't windy.
I want to try and apply a PI loop to this portion of Jason's DCM code, but I am not sure what to use for a set point or an input for the loop.
CrossVector( @Gravity, @Ref[3], @ErrorAxis[0] )
'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]
I believe how this should work is that the Erroraxis should be the input to the PI loop and that the set point should be zero. I think to start out I am going to use just a proportional loop.
The other thing that I am not sure about is whether I need a PI loop for each ErrorAxis variable or one for all 3. I kind of think 1 loop for each would be better.
Jason, have you added a PI loop for this in your new code?
I haven't added a PI loop for this yet, no. One variable for all three together kind of makes sense, but it'll be a little harder to write. Basically you'll have to measure the magnitude of the error (length of the vector) and feed that in as the input with zero as the set point. The output of the PI loop would be a scale that you apply to the input value to get the correction factor. (That's how I'd try it to start with)
So if I am understanding how the code works, if there is no error, then ErrorAxis[0], ErrorAxis[1] and ErrorAxis[2] would all be zero. I was under the assumption that these three variables are magnitudes of error, one for each of the 3 axis, X,Y and Z. Am I correct in this thinking?
By tweaking things I have found values for the code below that give great results. However there are two different extremes, for instance 9 works great when the quad is flying around, but during auto-level it is a little to low of a value. For auto-level a value of 11 works great, but after flying around a little and returning to auto-level there are noticeable effects.
CrossVector( @Gravity, @Ref[3], @ErrorAxis[0] )
'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]
Since shifting 8 bits to the right is like dividing by 256 I want to make a min and max output for my PI loop. 256 min which is shift 8. 2048 max which is shift 11.
It would almost have to be a reverse acting PI loop I think, if the error is great I want to use 256 and if the error is low I want to use 2048.
The more I think about this I maybe way off on how I think this loop should work.
The lower the value in the code above, the faster the error is corrected for. The higher the number the slower the error is corrected for.
AHHH, my brain is stuck again, that's what happens when I get on the parallax forum.
Why is 9 too low for auto-level? I'm curious how it responds - if that works well when flying around, it should also work well when sitting still.
Something I've seen done on other quads is ignore the auto-correction code unless the gravity vector is close to 1G. You can play with how close, but basically it means that if the quad is accelerating or decelerating hard, it won't be using that value to measure error, because it assumes it'll be wrong. It might help get rid of the settling that happens when you switch back to hovering after flying around. Otherwise you'll need to keep a lower value in that shift in order for the quad to recover from errors introduced by sudden accel / decel during flight.
With a value below 9 the quad seems to have a slight wobble, it is very slight. If I increase this value the wobble goes away. I assume that this is because of noise from the acc, but maybe I am wrong. The quad auto levels well with a value of 9 or 8 or even 11 for that matter. With a value of 11 the there is no wobble or oscillation during auto level.
I played with this value in your visualization program with just a proto board on a table. If I change the 8 to 0 the box on the screen jitters and bounces. It seems like the lower the value the faster the program reacts to errors, or the more weight the acc adds to the overall orientation.
I got rid of the acc input during a flight and used just the gyro. The DCM code handled everything pretty well, the quad was flyable for almost 5 minutes before it lost orientation to the point where I could not fly it. There was no wobble or oscillation.
The quad is flying very well, I am nit picking and trying to learn at the same time.
It seems that by increasing this value I can increase my roll and yaw PID values without getting oscillation, I am trying to tune this thing better. There are so many variables to be adjusted it is ridiculous. And then understanding how they all affect each other, it is quite a daunting task.
I have not flown my multiwii quad for awhile but I think the propeller powered quad is flying smoother than I had the multi wii flying. I got frustrated with the multi wii board because there were so many things to tweak, and my personal opinion is that it is hard to tweak something to perfection if you don't know how it works, and I had no idea how the code worked in the multi wii.
This has been one of the funnest projects I have ever worked on. Thanks for helping me Jason, and everyone else.
I have been playing with settings this morning to try and smooth out my quads flight. I started playing with the low pass filter I have for my ACC. I came up with some interesting results, I made the filter stronger and weaker and saw no difference in performance. So I turned the lowpass filter off all together and the quad flew the same as with it on. This got me to thinking.
I think the axis error correction in Jason's code actually acts like a low pass filter for the ACC. These are the 3 lines of code I believe act like a low pass filter.
Am I right in thinking this?
The code above that measures the error between the "measured" down (the accelerometer) and the "estimated" down (the matrix), and comes up with the ErrorAxis value. That error is then divided by 256, and used as a correction factor. That divide has a similar effect to averaging out the most recent 256 error measurements.
Comments
Thanks
Shawn
http://forums.parallax.com/showthread.php/149557-QuadCopter-Build-(My-Rambling-Thoughts-Questions-and-Comments)
To do this I first have to calculate the error.
To find the error I will take the transpose of row 1, and take it times row 2. This should give me the error. Doing this I believe is finding the DOT product of the 2 rows.
The next step would be to give half of this error to each the X and Y row and then rotate them in the opposite direction by cross coupling.
Now row X and Y should be orthogonal again.
To make row Z orthogonal again I would simply take the cross product of rows X and Y.
The final step of the re-normalization proccess would be to scale the rows so that they have a magnitude of 1.
And now I just confused myself again. I think I stripped the right therory out of the Premerlani and Bizard paper to this point, but I need to refresh myself with the dot and cross products again. And then figure out how to convert the math to spin.
I want to try and apply a PI loop to this portion of Jason's DCM code, but I am not sure what to use for a set point or an input for the loop.
I believe how this should work is that the Erroraxis should be the input to the PI loop and that the set point should be zero. I think to start out I am going to use just a proportional loop.
The other thing that I am not sure about is whether I need a PI loop for each ErrorAxis variable or one for all 3. I kind of think 1 loop for each would be better.
Jason, have you added a PI loop for this in your new code?
Thanks
Shawn
By tweaking things I have found values for the code below that give great results. However there are two different extremes, for instance 9 works great when the quad is flying around, but during auto-level it is a little to low of a value. For auto-level a value of 11 works great, but after flying around a little and returning to auto-level there are noticeable effects.
CrossVector( @Gravity, @Ref[3], @ErrorAxis[0] ) '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]Since shifting 8 bits to the right is like dividing by 256 I want to make a min and max output for my PI loop. 256 min which is shift 8. 2048 max which is shift 11.
It would almost have to be a reverse acting PI loop I think, if the error is great I want to use 256 and if the error is low I want to use 2048.
The more I think about this I maybe way off on how I think this loop should work.
The lower the value in the code above, the faster the error is corrected for. The higher the number the slower the error is corrected for.
AHHH, my brain is stuck again, that's what happens when I get on the parallax forum.
Something I've seen done on other quads is ignore the auto-correction code unless the gravity vector is close to 1G. You can play with how close, but basically it means that if the quad is accelerating or decelerating hard, it won't be using that value to measure error, because it assumes it'll be wrong. It might help get rid of the settling that happens when you switch back to hovering after flying around. Otherwise you'll need to keep a lower value in that shift in order for the quad to recover from errors introduced by sudden accel / decel during flight.
I played with this value in your visualization program with just a proto board on a table. If I change the 8 to 0 the box on the screen jitters and bounces. It seems like the lower the value the faster the program reacts to errors, or the more weight the acc adds to the overall orientation.
I got rid of the acc input during a flight and used just the gyro. The DCM code handled everything pretty well, the quad was flyable for almost 5 minutes before it lost orientation to the point where I could not fly it. There was no wobble or oscillation.
The quad is flying very well, I am nit picking and trying to learn at the same time.
It seems that by increasing this value I can increase my roll and yaw PID values without getting oscillation, I am trying to tune this thing better. There are so many variables to be adjusted it is ridiculous. And then understanding how they all affect each other, it is quite a daunting task.
I have not flown my multiwii quad for awhile but I think the propeller powered quad is flying smoother than I had the multi wii flying. I got frustrated with the multi wii board because there were so many things to tweak, and my personal opinion is that it is hard to tweak something to perfection if you don't know how it works, and I had no idea how the code worked in the multi wii.
This has been one of the funnest projects I have ever worked on. Thanks for helping me Jason, and everyone else.
I think the axis error correction in Jason's code actually acts like a low pass filter for the ACC. These are the 3 lines of code I believe act like a low pass filter.
Am I right in thinking this?
ErrorAxis[0] ~>= 8
ErrorAxis[1] ~>= 8
ErrorAxis[2] ~>= 8
The code above that measures the error between the "measured" down (the accelerometer) and the "estimated" down (the matrix), and comes up with the ErrorAxis value. That error is then divided by 256, and used as a correction factor. That divide has a similar effect to averaging out the most recent 256 error measurements.