The complimentary filter will slowly correct the problem, but it won't "track" properly. What you're seeing is the non-commutative nature of rotations, and the fact that you're comparing the orientation of the gyro to the ground, where the ground doesn't move. The gyro only knows at any given instant how fast it's rotating around one of its own axis. It has no idea how those axis relate to the ground. If you pitch forward 45 degrees, your Z (down) axis is no longer in alignment with the ground's Z axis, so a 90 degree rotation around your gyro's Z isn't the same as a 90 degree rotation around the ground's Z.
The DCM system (and quaternion-based IMUs) solve this by tracking the orientation of the ground relative to that of the gyro. As the gyro orientation changes, they keep track of the new orientation relative to the ground, and rotate THAT orientation using the new incoming gyro readings. It's iterative - the DCM code makes minuscule adjustments to the "world orientation" matrix every update loop.
Don't let the word "matrix" freak you out, either - It's three vectors of unit length: Left, Up, and Forward. The math is a little heavy, but it's not that bad. Do some reading on rotation matrices, then take a look at the DCM paper. I had to read it about 15 to 20 times before it sunk in enough to actually code it, so if you're a little overwhelmed, you're in good company.
Just out of curiosity Jason do you have an attitude program working on a quad(probably the DMC)? I think I read somewhere that you said you did. If so, how is it working for you? One other interesting thing that I noticed is that I am not really getting any drift out of my Z axis (yaw) on my gyro. It has been sitting on my desk for a half hour and has drifted from .000 to .005 degrees. I am not sure why I can't get the same results out of the X and Y axis's.
The other interesting thing is that by switching from floating point math to integer math I have cut the time down from 3mS to 1mS for all of my angle calculations.
I do have the DCM stuff working on a quad, but I've been having issues with vibration on the frame screwing it up. I assume that's what it is - The one frame I had that didn't have vibration issues it was working really well, and then I augered it into the pavement doing something stupid. Using the same code on new setups, I haven't gotten it anywhere near as stable as it was, and I *think* it's from too much motor or prop vibration.
Regarding your Z-axis, gyros have different sensitivity and different drift on the various axis. It might be temperature drift that's causing your "sliding", especially if you're using my gyro code. I think I set it up to counter the measured drift on my own gyro, which will be different than yours. It will change very slowly, but it'll be noticeable if you leave the board sitting for a while.
And integer math is the way to go - especially given that the Prop has to emulate floating point math.
Update on my progress.
Everything is converted over to integers, gyro reading and angle calculation, acc reading and angle calculation, complimentary filter and the PID loops. I know I have said that I am about ready to fly this thing for a couple weeks but every time I start writing the code I stumble across something I want to change. The only thing I have to figure out now, is how to implement the code for yaw control and I think I know how I am going to do that.
The biggest time eater right now for me will be mounting the board and building a stand to test the quad on.
I also have been trying to figure out how to find a surface that is completely level so I can calibrate my acc. I think I am going to calibrate the acc while the board is off the quad, I am going to either use a aquarium filled with water and a big piece of foam to float in the tank and put my board on or I am going to build a frame and fill it with some self leveling compound.
I will post the integer code for the complimentary filter when I get it cleaned up and commented.
The Quad code is written and the copter is put together, I am just waiting for some prop adapters. I got 1 question while I am waiting for parts. Are any of you guys using a compass or magnetometer to feed into a complimentary filter with the yaw gyro reading? If so which one are you guys using? I would like to incorporate heading hold into this version of my quad.
Well I decided to take the props off of my other quad and try my program. It failed miserably, I played with it for 4 or 5 hours, so many things that need tweaked. I need to try something different with my PID loops I think and also filter my ACC better. I get terrible oscillation when trying to lift off the ground. I set my D and I terms to zero and my P term at 1 and still get oscillation. It oscillates for may be a second or two and then tries to flip. Can a noisy ACC cause this or does this sound like more of a PID problem.
Thanks
Shawn
Oscillation is probably a PID issue. If you're doing it all with integer math, you'll likely have to scale down quite a bit somewhere. The range of inputs to your ESCs is 1000 to 2000, meaning only 1000 unique values between "off" and "full throttle". A change of 50 represents a decent jump. Dump the output of your PIDs to a serial terminal and watch how they respond as you move the quad. If they're outputting large numbers, you'll probably need to scale the results down before you use them, either by shifting or dividing.
Hey guys, I have been looking at my PID loops and I can not find anything wrong with them. At least the P section anyways. I have added a low pass filter for my ACC. I figured in my head that I did not need one because the complimentary filter in it self is a low pas filter for the ACC. However it dawned on me that it might be a good idea to smooth out that ACC reading before it is dumped into the complimentary filter. Also I have the 6DOF board from spark fun which has the solder pins on only one edge of the breakout board leaving the other edge just hanging there. I think the board could be vibrating a lot. I have my PID loops set so that the max output can only add 50 to my motor outputs.
The only other thing that I can think of, is that my servo control part of my control is running the PMW signal at 400hz, when I started messing with this project I thought I was going to be able to run through my flight loop faster. Is it possible that 400hz is to fast, and the ESC's are not actually updating fast enough?
Wow I just found 1 problem what do you guys think about this? After I switched everything over to integers I changed my flight loop to run at 250Hz instead of 200Hz, but I forgot to change my dT in my gyro angle calculation from 5mS to 4mS? I don't think that would have caused oscilation problems but I am not sure what do you guys think?
Jason do you run your ACC through a low pass filter in your DCM code?
Thanks
Shawn
Hey Jason are you still using your ITG320 PASM Sport program to read your gyro and acc? While I am waiting to do some work to the frame on my quad I have been thinking about trying to speed up my sensor reading times. How fast do you think the ITG3200 Sport would run? I am thinking about changing this line.
loopDelay long 80_000_000/250 '250hz
to this
loopDelay long 80_000_000/400 '250hz
The reason why I am thinking about this is because I want to keep my 250Hz flight loop and I want to try and filter the gyro also. I think it is possible to keep the 250Hz flight loop if I am reading the gyro even faster or am I wrong?
Also I am not sure how one does a high pass digital filter. Is it the same principal as the low pass filter you guys helped me with? Or can one not filter the gyro since I am using a specif dT in my gyro angle calculation? In my mind as long as I am sampling faster than my dT, I should be able to filter it.
Another assumption of mine is that if I am reading my ACC faster, then I should be able to get better filtering and reduce the latency that the filtering adds.
Thanks
Shawn
I think the board could be vibrating a lot. I have my PID loops set so that the max output can only add 50 to my motor outputs.
Put some double-sided foam tape, or something else like that under it. You can actually buy vibration damping foam that companies like Futaba ship with their gyros. I've used that before. Not sure how much of a difference it makes, but it can't hurt. As for the motor limit, there's a BIG difference between limiting and scaling. For example, draw a sine wave from -1 to +1. Now cut off everything over 0.5. That's limiting (also called clipping). Now take that same sine wave and multiply all the values by 0.5. That's scaling. You may need to scale the output of your PIDs so that the shape is the same, but the maximum output value is lower.
The only other thing that I can think of, is that my servo control part of my control is running the PMW signal at 400hz, when I started messing with this project I thought I was going to be able to run through my flight loop faster. Is it possible that 400hz is to fast, and the ESC's are not actually updating fast enough?
It's possible, but most likely they'd just ignore the extra signals.
After I switched everything over to integers I changed my flight loop to run at 250Hz instead of 200Hz, but I forgot to change my dT in my gyro angle calculation from 5mS to 4mS? I don't think that would have caused oscilation problems but I am not sure what do you guys think?
More likely that it'd cause "settling" at the wrong angle values, then the acc would pull it back over time.
Jason do you run your ACC through a low pass filter in your DCM code?
Yup - It actually made a really big difference, though I suspect it's because my DCM code doesn't maintain enough precision in the acc contributions to do proper filtering there.
Hey Jason are you still using your ITG320 PASM Sport program to read your gyro and acc? While I am waiting to do some work to the frame on my quad I have been thinking about trying to speed up my sensor reading times. How fast do you think the ITG3200 Sport would run? I am thinking about changing this line.
loopDelay long 80_000_000/250 '250hz
to this
loopDelay long 80_000_000/400 '250hz
The reason why I am thinking about this is because I want to keep my 250Hz flight loop and I want to try and filter the gyro also. I think it is possible to keep the 250Hz flight loop if I am reading the gyro even faster or am I wrong?
If you're going to read the gyro / acc at higher speeds, you'd need to read the results out at 400hz, and accumulate them, filter them, then pass them to your main loop after filtering. If the main loop is only reading the output at 250 hz, it won't matter how fast the gyro is being read. You need to do something with the results. The first iteration of my ITG3200 code took gyro readings 2000 times per second, added them together and divided by 8 before passing that down to my main loop. It seemed to help. On the other hand, the ITG3200 is capable of doing this by itself in the hardware, which is why I took it out. I haven't done enough testing to figure out if doing it "manually" produces a better result or not.
I'm not sure how fast the code will go if it's reading both the gyro and the acc, but it should be able to handle 400/sec with no trouble.
Thanks for the replys Jason.
That makes perfect sense what your saying about clamping the sine wave versus scaling the sine wave(PID Output). Some how I am just not getting it, isn't the P value of the PID loop basically scaling the output in itself. P is multiplied by the difference in desired set point versus actual set point, or at least that is how I understand it. As I increase the P value the oscillation becomes worse. The problem is I can decrease the P value all the way down to almost nothing and I still get oscillation. If I put P to zero I obviously get no oscillation and no attitude control. With my P value at 1 I still got oscillation to the point the quad would eventually flip. So I clamped my max out put of my PID loop to 20 to see if I could keep the quad from flipping, but it still flipped. I wrote a simple vb program so I could see all of my PID values and angles and what not, and everything seems to track ok when I have the board in my hand and I tilt it. Could latency in my complimentary filter cause oscillation? I have seen this problem before in my other quad and I needed to balance my props because they were vibrating to much. I am hoping that securing my 6DOF board and running the ACC through a high pass filter will help, but I think you are right there has to be a problem with my PID. The family just got home so I can't ramble on any more. I think I am going to start a new thread for the PID disscusion this is kind of getting away from the complimentary filter.
Thanks
Shawn
Thanks for the replys Jason.
Some how I am just not getting it, isn't the P value of the PID loop basically scaling the output in itself. P is multiplied by the difference in desired set point versus actual set point, or at least that is how I understand it.
That's exactly right, but you may still have an issue if your P value needs to be fractional and you're using integer math.
Let's say your gyro has an output range of +/- 2000, and your motor controller takes inputs in the range of +/- 500 (1000 to 2000, centered at 1500 gives you +/- 500). In this case, no amount of integer scaling is going to give you outputs that are within the range of your motor controller. The P value '1' will generate outputs of +/- 2000, 4x greater than your motor controller wants. Since you're using integers, you can't go lower than 1.
So you use fixed point math. Assume your 'P' value is a fixed point number, with 8 fractional bits, and do it like this:
Output := ((SetPoint - Measured) * P) / 256
So now, if P is the value '256', it cancels out the divide on the end. If P is a value of 128, you get the same output as you would if you were doing the work in floating point and P was 0.5.
You can choose an arbitrary scale - I used 8 bits, but you can use more or less. Whatever you use as the divisor is your scale. Setting P to the same value as the divisor is the equivalent of a P value of 1.0. Try that and see what happens. There are slightly better ways to do the scaling, but start with this because it's easy to wrap your head around.
Jason that actually clears a lot up for me thank you.
In your hoverfly sport program you shifted values a lot by 8 and I knew you were scaling but I did not understand how it worked. Shifting 8 is like dividing by 256 right. So in your example in post #75, if I made P equal to 512 then that would be like making P 2 in floating point, is that correct?
Shawn
Yup - exactly right. << is "shift left", or "multiply by 2, N times". >> is "shift right, unsigned", and ~> is "shift right, respecting the sign bit" (divide by 2, N times). If you use ~> 8 instead of / 256, it's faster.
Ok test flight number 2 failed also.
I added acc filter and secured my 6DOF board better in between test flight 1 and 2. Jason do you filter your gyro readings also? Would the gyro be sent through a low pass digital filter a high pass? I do not think the problem is my PID, there is to much noise coming from the sensors. I have used this this quad frame with a quadrino zoom flight controller with no major problem. When my quad is sitting on the ground and I start my motors the angle readings coming from my complimentary filter jump around pretty bad. I don't think this is right. They jump up and down by almost 1 or 2 degrees.
Thanks
Shawn
You can filter the gyro readings, but if you can reduce the noise in other ways it's better to do that first. Low pass = low-frequency information gets passed through (think slow-ish changing stuff). High-pass = high-frequency information gets through (like noise). So you want a low-pass filter.
Also, try removing the acc from it completely and see if that helps. It's possible that your acc, when not in the actual flying quad, is well behaved, but becomes really jumpy when the motors are running. You may need more filtering on it than you have.
Hey everyone,
Its been awhile since I have posted. My test flight a week or so did not turn out to well and I bent all four of my motor shafts. I finished rebuilding my quad and adding some noise damping rubber under my motor mounts. I also played with the ACC filtering and gyro filtering. I held down the quad tonight and ramped the throttle to full to see if my angle readings jumped around. They jumped around about 2 to 3 tenths of a degree. I thought this was pretty good. I still can not get a hover out of the quad. I get terrible oscillation with higher PID settings and lower PID setting I get terrible drift and no response out of my stick input. My stick input is used to calculate a new desired angle. Set my complimentary filter to completely block out my acc and I still get terrible oscillation with higher PID settings and bad drift with lower settings. I think maybe I need to trim my quad out but I can't do this with the transmitter.
Working through this problem in my head an unbalanced quad could cause these results. The quad wants to drift and if I do not have enough P the motors will never speed up enough to level the quad. If the motors can not speed up enough to level the quad applying stick input in the opposite direction will not have any effect either. Now if I increase my P the motors will speed up enough to level the quad but begin to oscillate because that is just what happens when P is to high. What do you guys think about that?
I am only using the P value right now in my PID, and I believe I do have my PID values scaled accordingly.
Thanks
Shawn
Try this: Hold your quad in your hand, at arms length, and spin the motors up to about 1/4. (hold on tight - it's dangerous) Then, tilt the quad in a direction and feel how it responds. If it doesn't "fight you" basically immediately, it'll never fly. If it takes a long time to respond, there's something wrong in your code. The quad needs to get angular readings basically instantly, and respond to them instantly as well. If either of those isn't happening, you'll never get it to fly.
This is exactly how I tuned mine - I just held onto it, and felt how it responded. When I got it to the point where I could feel it resist me trying to move it, and the response to the stick inputs was correct, I took it outside, but not before. It might also be worth trying a tethering rig - Tie it to some string or rope, tightly suspended between two chairs, from both sides of the quad. Spin up the motors and see how it responds. You'll only be able to test it along one axis, but that'll be enough to tell if it's right or not.
Check that when you give it stick input you're changing it enough (and not too much) and check that it's sending decent motor output in response to being moved. If you can, feed output from your "active" axis to the serial port (watch the cable doesn't get caught in the props). It takes a while to get right.
Ya I will have to build a stand. Meanwhile I would like to start posting pieces of my code and have you guys look at it, that is if you guys would be willing too. I think there is a problem with my PID loops but I am not sure.
I am scaling my stick input to degrees, right now I have it maxing out at 6 degrees. If my PID's are leveling the quad with no stick input, then changing my desired angle with my stick input should cause the quad to change pitch or roll accordingly, to match the new desired angle.
My complimentary filter is outputting an integer. For instance if my output from my complimentary filter value for Roll is 1000 this means the quad is sensing a 1.000 degree tilt. If the Roll value is 45000 then the actual angle is 45.000 degrees.
Here is my PID code it takes my desired angle and subtracts it from my actual angle I am only using the P term right now. If I set P to 5 I get no control and no oscillation and if I set P to 15 I get oscillation and little control.
That may not be enough info to judge or get a gist of what I am doing. The link below is the whole program, IT DOES NOT WORK so please do not try to use it. Also it is not commented as well as I would like. If anyone would like to look at it I would be appreciative.
I was playing with the quad at lunch a little, is it possible that the P term alone will not be enough in the PID loop to level the quad? I added some I to the PID and it seems a little better but I cannot really get it off the ground, I am going to build a stand tonight. I have read multiple times that a PD controller is all that is needed to hover a quad. I tried to add some D but my PID has a problem apparently so I added I to see what would happen.
Thanks
Shawn
Actually lots of people just use P (it won't perfectly level, but it's mostly stable), and PI or PD can also be made to work quite easily.
You can also use the P term with your angular difference, and feed the gyro values in directly for the D term, since that's what the gyro is measuring anyway - it's the rate of change of that value. It's a lot to wrap your head around. Build the test rig and you'll be able to play with the numbers "live", which will help you get a feel for what things are doing. You'll also be able to make it output what it's reading & responding with to the serial terminal, which may help you discover bugs in the code.
I have a simple VB program that I have a single cog dedicated to dumping out data to the PC. Everything looks like it is working properly when I hold the board in my hand and I tilt it. I am going to play with the I portion of my PID and see if I can smooth it out. I do think part of my problem is the PID Loop it looks good to me but something is a little bit off, I basically copied it from the integer PID object in the exchange.
What sounds reasonable, if I have my P value set to 10 and my I and D value set to 0 for every degree of tilt my board is off the PID loop only adds or subtracts 1 to my motor speeds. This does not seem to be much but if I raise the value much more I get oscillation.
What is reasonable for PID output if the quad is 1 degree off, 10us, 1us, 50us, I have no idea if I am in the ballpark?
Also, can to much filtering on the ACC cause a lag which would cause oscillation during flight?
I am filtering my ACC very heavily but in my complimentary filter I am using 99 percent gyro data and 1 percent ACC data.
Filtering on the acc will cause some lag, but the acc is ONLY used to level the quad, not hold it steady - that's what the gyro is for. The gyro will give you a reading if you're tilting, which the PID should correct for. How are you getting the angle values you're using, and what's their resolution? My quad, for example, just accumulates the raw gyro values from one update to the next, so I'm getting obscenely high numbers, but it also means that I can react to *very* small changes in angle. If your numbers don't have enough resolution, you won't be able to read a tilt speed of a couple degrees per second, for example, which means your PID won't do anything until you're pretty far off level.
I am using an ITG3200 and an ADXL345. Everything in my program takes place in a 250Hz flight loop. First I read my Gyro data and then I calculate an angle like this.
PUB Convert_GYRO_2_Angle
'>>>>>>>>>>> This Section Takes uS
Gx := Gx * 4
Gx := GX * 1000
Gx := GX / 14375
GRoll := Roll + Gx
Then I read my ACC and convert it to an angle like this.
PUB Convert_ACC_2_Angle
'>>>>>>>>>>> This Section Takes uS
Ax := GyroAcc.getArx 'Read ACC Angle X from Cordic Table in ITG 3200 Sport OBJ
Ax := Ax * 5493 'ITG OBJ puts out value 0 to 65535 which is 0 to 360 degrees. 360/65535 =.00549324
ARoll := Ax / 1000 'I want 3 decimal places of resolution.
If ARoll > 180000 'I do not want 0 to 360 I want 0 to 180 and 0 to -180.
ARoll := ARoll - 360000
ARoll *= -1 'This Axis needs inverted to match my gyro.
ARoll := ARoll + Ax0 'Ax0 is Acc offset to make the ACC read zero when level or at least my guess of what 0 is.
Then I filter my ACC reading like this
PUB LowPass_Filter_ACC
'>>>>>>>>>>> This Section Takes uS
ARoll := ARollLFS + ((AlphaACC * (ARoll - ARollLFS))/1000)
ARollLFS := ARoll
Then I run my ACC and Gyro calculated angles through the Complimentary Filter
PUB Complimentary_Filter
'>>>>>>>>>>> This Section Takes uS
'>>>>>>>>>>> This section is the first complimentary filter that I have tried the section below will be a different one.
'>>>>>>>>>>> Roll = (GRoll * GYROPCT) + (ARoll * ACCPCT)
Roll := ((GRoll * GPCT)+(ARoll * APCT))/ 1000 'was 950 and 50 now APCT is 1 and GPCT is 999
Then I change my RC signal in to a desired setpoint in degrees. Right now it is set with a range of -12.000 degrees to 12.000 degrees
Then I send my desired set point (Angle) from my RC input, and the quads measured angle into my PID loops. I have a seperate PID for Roll, Pitch, and Yaw. The code snippets I post above are only for my Roll axis.
This is my Roll PID.
I think I have a resolution of 3 decimal places but I may be wrong. I read the RC inputs and convert them to a range of -500 to 500, which is also the number I feed back to my servo program. Hopefully you can understand my code. I have to scale down my PID loop or I constantly get oscillation no mater what my P value is. Right now I and D are both 0 although this afternoon I did play with the I value.
Thanks
Shawn
Your ARoll value looks fine - You're computing a number from -180,000 to +180,000, and that should be ok. The GRoll value might be the issue though.
You're taking GX * 4000 / 14375, which means that if GX is < 4, you won't see any result at all. You may be getting a lot of lost precision by doing the divide before you do the accumulation. If I were you, I'd simply be computing a running sum of the Gx values, and then making *that* human readable by doing the scaling. You won't lose that precision forever.
I ran through your other math and it looks to all be correct.
*edit* - Never mind - I see where that Roll value is calculated, but it still means you're losing precision by throwing out the lower few bits of Gx.
Hey Jason,
Thanks for the reply. My flight loop is 250Hz which is 4 ms. The 4000 is the DT value for calculating my angle from the gyro. How can I get but without using the 4ms. My thought process on the 4000 was to make everything an integer. I think the equation for determining an angle from the ITG3200 is..............
Gx = (Gx * Dt) / 14.375 LSBs
Roll = Roll + Gx
I think I am still not understanding something.
What did you think about the PID loop? It may not have been commented enough to understand.
Thanks
Shawn
Oh I see what you mean, 4 * 4000 = 16000
3 * 4000 = 12000
12000 / 14375 is a fraction and returns 0
Still though how can I get by with out using the 4000 and calculate an accurate angle? I am missing something with accumulating the gyro reading I think.
Jason
Is the same thing happening in my PID loop? I am dividing the PID output by 10000 to try and scale down the output to my motors but anything less than 10000 will be lost. Hmmm the old brain matter is a churning but I don't quite have it.
Comments
http://jmonkeyengine.org/wiki/doku.php/jme3:math
The DCM system (and quaternion-based IMUs) solve this by tracking the orientation of the ground relative to that of the gyro. As the gyro orientation changes, they keep track of the new orientation relative to the ground, and rotate THAT orientation using the new incoming gyro readings. It's iterative - the DCM code makes minuscule adjustments to the "world orientation" matrix every update loop.
Don't let the word "matrix" freak you out, either - It's three vectors of unit length: Left, Up, and Forward. The math is a little heavy, but it's not that bad. Do some reading on rotation matrices, then take a look at the DCM paper. I had to read it about 15 to 20 times before it sunk in enough to actually code it, so if you're a little overwhelmed, you're in good company.
I do have the DCM stuff working on a quad, but I've been having issues with vibration on the frame screwing it up. I assume that's what it is - The one frame I had that didn't have vibration issues it was working really well, and then I augered it into the pavement doing something stupid. Using the same code on new setups, I haven't gotten it anywhere near as stable as it was, and I *think* it's from too much motor or prop vibration.
Regarding your Z-axis, gyros have different sensitivity and different drift on the various axis. It might be temperature drift that's causing your "sliding", especially if you're using my gyro code. I think I set it up to counter the measured drift on my own gyro, which will be different than yours. It will change very slowly, but it'll be noticeable if you leave the board sitting for a while.
And integer math is the way to go - especially given that the Prop has to emulate floating point math.
Everything is converted over to integers, gyro reading and angle calculation, acc reading and angle calculation, complimentary filter and the PID loops. I know I have said that I am about ready to fly this thing for a couple weeks but every time I start writing the code I stumble across something I want to change. The only thing I have to figure out now, is how to implement the code for yaw control and I think I know how I am going to do that.
The biggest time eater right now for me will be mounting the board and building a stand to test the quad on.
I also have been trying to figure out how to find a surface that is completely level so I can calibrate my acc. I think I am going to calibrate the acc while the board is off the quad, I am going to either use a aquarium filled with water and a big piece of foam to float in the tank and put my board on or I am going to build a frame and fill it with some self leveling compound.
I will post the integer code for the complimentary filter when I get it cleaned up and commented.
Shawn
Shawn
Thanks
Shawn
Jason
The only other thing that I can think of, is that my servo control part of my control is running the PMW signal at 400hz, when I started messing with this project I thought I was going to be able to run through my flight loop faster. Is it possible that 400hz is to fast, and the ESC's are not actually updating fast enough?
Wow I just found 1 problem what do you guys think about this? After I switched everything over to integers I changed my flight loop to run at 250Hz instead of 200Hz, but I forgot to change my dT in my gyro angle calculation from 5mS to 4mS? I don't think that would have caused oscilation problems but I am not sure what do you guys think?
Jason do you run your ACC through a low pass filter in your DCM code?
Thanks
Shawn
loopDelay long 80_000_000/250 '250hz
to this
loopDelay long 80_000_000/400 '250hz
The reason why I am thinking about this is because I want to keep my 250Hz flight loop and I want to try and filter the gyro also. I think it is possible to keep the 250Hz flight loop if I am reading the gyro even faster or am I wrong?
Also I am not sure how one does a high pass digital filter. Is it the same principal as the low pass filter you guys helped me with? Or can one not filter the gyro since I am using a specif dT in my gyro angle calculation? In my mind as long as I am sampling faster than my dT, I should be able to filter it.
Another assumption of mine is that if I am reading my ACC faster, then I should be able to get better filtering and reduce the latency that the filtering adds.
Thanks
Shawn
Put some double-sided foam tape, or something else like that under it. You can actually buy vibration damping foam that companies like Futaba ship with their gyros. I've used that before. Not sure how much of a difference it makes, but it can't hurt. As for the motor limit, there's a BIG difference between limiting and scaling. For example, draw a sine wave from -1 to +1. Now cut off everything over 0.5. That's limiting (also called clipping). Now take that same sine wave and multiply all the values by 0.5. That's scaling. You may need to scale the output of your PIDs so that the shape is the same, but the maximum output value is lower.
It's possible, but most likely they'd just ignore the extra signals.
More likely that it'd cause "settling" at the wrong angle values, then the acc would pull it back over time.
Yup - It actually made a really big difference, though I suspect it's because my DCM code doesn't maintain enough precision in the acc contributions to do proper filtering there.
If you're going to read the gyro / acc at higher speeds, you'd need to read the results out at 400hz, and accumulate them, filter them, then pass them to your main loop after filtering. If the main loop is only reading the output at 250 hz, it won't matter how fast the gyro is being read. You need to do something with the results. The first iteration of my ITG3200 code took gyro readings 2000 times per second, added them together and divided by 8 before passing that down to my main loop. It seemed to help. On the other hand, the ITG3200 is capable of doing this by itself in the hardware, which is why I took it out. I haven't done enough testing to figure out if doing it "manually" produces a better result or not.
I'm not sure how fast the code will go if it's reading both the gyro and the acc, but it should be able to handle 400/sec with no trouble.
That makes perfect sense what your saying about clamping the sine wave versus scaling the sine wave(PID Output). Some how I am just not getting it, isn't the P value of the PID loop basically scaling the output in itself. P is multiplied by the difference in desired set point versus actual set point, or at least that is how I understand it. As I increase the P value the oscillation becomes worse. The problem is I can decrease the P value all the way down to almost nothing and I still get oscillation. If I put P to zero I obviously get no oscillation and no attitude control. With my P value at 1 I still got oscillation to the point the quad would eventually flip. So I clamped my max out put of my PID loop to 20 to see if I could keep the quad from flipping, but it still flipped. I wrote a simple vb program so I could see all of my PID values and angles and what not, and everything seems to track ok when I have the board in my hand and I tilt it. Could latency in my complimentary filter cause oscillation? I have seen this problem before in my other quad and I needed to balance my props because they were vibrating to much. I am hoping that securing my 6DOF board and running the ACC through a high pass filter will help, but I think you are right there has to be a problem with my PID. The family just got home so I can't ramble on any more. I think I am going to start a new thread for the PID disscusion this is kind of getting away from the complimentary filter.
Thanks
Shawn
That's exactly right, but you may still have an issue if your P value needs to be fractional and you're using integer math.
Let's say your gyro has an output range of +/- 2000, and your motor controller takes inputs in the range of +/- 500 (1000 to 2000, centered at 1500 gives you +/- 500). In this case, no amount of integer scaling is going to give you outputs that are within the range of your motor controller. The P value '1' will generate outputs of +/- 2000, 4x greater than your motor controller wants. Since you're using integers, you can't go lower than 1.
So you use fixed point math. Assume your 'P' value is a fixed point number, with 8 fractional bits, and do it like this:
Output := ((SetPoint - Measured) * P) / 256
So now, if P is the value '256', it cancels out the divide on the end. If P is a value of 128, you get the same output as you would if you were doing the work in floating point and P was 0.5.
You can choose an arbitrary scale - I used 8 bits, but you can use more or less. Whatever you use as the divisor is your scale. Setting P to the same value as the divisor is the equivalent of a P value of 1.0. Try that and see what happens. There are slightly better ways to do the scaling, but start with this because it's easy to wrap your head around.
In your hoverfly sport program you shifted values a lot by 8 and I knew you were scaling but I did not understand how it worked. Shifting 8 is like dividing by 256 right. So in your example in post #75, if I made P equal to 512 then that would be like making P 2 in floating point, is that correct?
Shawn
J
I added acc filter and secured my 6DOF board better in between test flight 1 and 2. Jason do you filter your gyro readings also? Would the gyro be sent through a low pass digital filter a high pass? I do not think the problem is my PID, there is to much noise coming from the sensors. I have used this this quad frame with a quadrino zoom flight controller with no major problem. When my quad is sitting on the ground and I start my motors the angle readings coming from my complimentary filter jump around pretty bad. I don't think this is right. They jump up and down by almost 1 or 2 degrees.
Thanks
Shawn
Also, try removing the acc from it completely and see if that helps. It's possible that your acc, when not in the actual flying quad, is well behaved, but becomes really jumpy when the motors are running. You may need more filtering on it than you have.
Its been awhile since I have posted. My test flight a week or so did not turn out to well and I bent all four of my motor shafts. I finished rebuilding my quad and adding some noise damping rubber under my motor mounts. I also played with the ACC filtering and gyro filtering. I held down the quad tonight and ramped the throttle to full to see if my angle readings jumped around. They jumped around about 2 to 3 tenths of a degree. I thought this was pretty good. I still can not get a hover out of the quad. I get terrible oscillation with higher PID settings and lower PID setting I get terrible drift and no response out of my stick input. My stick input is used to calculate a new desired angle. Set my complimentary filter to completely block out my acc and I still get terrible oscillation with higher PID settings and bad drift with lower settings. I think maybe I need to trim my quad out but I can't do this with the transmitter.
Working through this problem in my head an unbalanced quad could cause these results. The quad wants to drift and if I do not have enough P the motors will never speed up enough to level the quad. If the motors can not speed up enough to level the quad applying stick input in the opposite direction will not have any effect either. Now if I increase my P the motors will speed up enough to level the quad but begin to oscillate because that is just what happens when P is to high. What do you guys think about that?
I am only using the P value right now in my PID, and I believe I do have my PID values scaled accordingly.
Thanks
Shawn
This is exactly how I tuned mine - I just held onto it, and felt how it responded. When I got it to the point where I could feel it resist me trying to move it, and the response to the stick inputs was correct, I took it outside, but not before. It might also be worth trying a tethering rig - Tie it to some string or rope, tightly suspended between two chairs, from both sides of the quad. Spin up the motors and see how it responds. You'll only be able to test it along one axis, but that'll be enough to tell if it's right or not.
Like this, but only from two of the sides, not all four : http://www.youtube.com/watch?v=0OY62VZQCHc
Check that when you give it stick input you're changing it enough (and not too much) and check that it's sending decent motor output in response to being moved. If you can, feed output from your "active" axis to the serial port (watch the cable doesn't get caught in the props). It takes a while to get right.
Jason
I am scaling my stick input to degrees, right now I have it maxing out at 6 degrees. If my PID's are leveling the quad with no stick input, then changing my desired angle with my stick input should cause the quad to change pitch or roll accordingly, to match the new desired angle.
My complimentary filter is outputting an integer. For instance if my output from my complimentary filter value for Roll is 1000 this means the quad is sensing a 1.000 degree tilt. If the Roll value is 45000 then the actual angle is 45.000 degrees.
Here is my PID code it takes my desired angle and subtracts it from my actual angle I am only using the P term right now. If I set P to 5 I get no control and no oscillation and if I set P to 15 I get oscillation and little control.
PUB Initiate_PID_Values DoInt := False PIDMaxOutput := 50 PIDMaxIntegral := 2000 '(PIDMaxIntergral * Ki) / PIDOutFactor = How much will be added to the PIDOutTotal. KpRoll := 5 KiRoll := 0 KdRoll := 0 KpPitch := 5 KiPitch := 0 KdPitch := 0 KpYaw := 80 KiYaw := 0 KdYaw := 0 '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> PUB Roll_PID '>>>>>>>>>>> This Section Takes uS 'DbgCountLow := cnt PErrorRoll := DesiredRoll - Roll DErrorRoll := PErrorRoll - LstPErrorRoll LstPErrorRoll := PErrorRoll PIDRollOut := (KpRoll * PErrorRoll) + (KiRoll * IErrorRoll) + (KdRoll * DErrorRoll) PIDRollOut := PIDRollOut/10000 PIDRollOut := -PIDMaxOutput #> PIDRollOut <# PIDMaxOutput if(DoInt) IErrorRoll += PErrorRoll IErrorRoll := -PIDMaxIntegral #> IErrorRoll <# PIDMaxIntegral Debug[28] := PIDRollOut 'DbgCountHigh := cnt '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>That may not be enough info to judge or get a gist of what I am doing. The link below is the whole program, IT DOES NOT WORK so please do not try to use it. Also it is not commented as well as I would like. If anyone would like to look at it I would be appreciative.xquad 1.3.zip
Thanks
Shawn
Thanks
Shawn
You can also use the P term with your angular difference, and feed the gyro values in directly for the D term, since that's what the gyro is measuring anyway - it's the rate of change of that value. It's a lot to wrap your head around. Build the test rig and you'll be able to play with the numbers "live", which will help you get a feel for what things are doing. You'll also be able to make it output what it's reading & responding with to the serial terminal, which may help you discover bugs in the code.
What sounds reasonable, if I have my P value set to 10 and my I and D value set to 0 for every degree of tilt my board is off the PID loop only adds or subtracts 1 to my motor speeds. This does not seem to be much but if I raise the value much more I get oscillation.
What is reasonable for PID output if the quad is 1 degree off, 10us, 1us, 50us, I have no idea if I am in the ballpark?
Also, can to much filtering on the ACC cause a lag which would cause oscillation during flight?
I am filtering my ACC very heavily but in my complimentary filter I am using 99 percent gyro data and 1 percent ACC data.
Thanks
Shawn
Then I read my ACC and convert it to an angle like this.
PUB Convert_ACC_2_Angle '>>>>>>>>>>> This Section Takes uS Ax := GyroAcc.getArx 'Read ACC Angle X from Cordic Table in ITG 3200 Sport OBJ Ax := Ax * 5493 'ITG OBJ puts out value 0 to 65535 which is 0 to 360 degrees. 360/65535 =.00549324 ARoll := Ax / 1000 'I want 3 decimal places of resolution. If ARoll > 180000 'I do not want 0 to 360 I want 0 to 180 and 0 to -180. ARoll := ARoll - 360000 ARoll *= -1 'This Axis needs inverted to match my gyro. ARoll := ARoll + Ax0 'Ax0 is Acc offset to make the ACC read zero when level or at least my guess of what 0 is.Then I filter my ACC reading like this
Then I run my ACC and Gyro calculated angles through the Complimentary Filter
Then I change my RC signal in to a desired setpoint in degrees. Right now it is set with a range of -12.000 degrees to 12.000 degrees
Then I send my desired set point (Angle) from my RC input, and the quads measured angle into my PID loops. I have a seperate PID for Roll, Pitch, and Yaw. The code snippets I post above are only for my Roll axis.
This is my Roll PID.
PUB Roll_PID '>>>>>>>>>>> This Section Takes uS 'DbgCountLow := cnt PErrorRoll := DesiredRoll - Roll DErrorRoll := PErrorRoll - LstPErrorRoll LstPErrorRoll := PErrorRoll PIDRollOut := (KpRoll * PErrorRoll) + (KiRoll * IErrorRoll) + (KdRoll * DErrorRoll) PIDRollOut := PIDRollOut/10000 PIDRollOut := -PIDMaxOutput #> PIDRollOut <# PIDMaxOutput if(DoInt) IErrorRoll += PErrorRoll IErrorRoll := -PIDMaxIntegral #> IErrorRoll <# PIDMaxIntegral Debug[28] := PIDRollOut 'DbgCountHigh := cnt '>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>I think I have a resolution of 3 decimal places but I may be wrong. I read the RC inputs and convert them to a range of -500 to 500, which is also the number I feed back to my servo program. Hopefully you can understand my code. I have to scale down my PID loop or I constantly get oscillation no mater what my P value is. Right now I and D are both 0 although this afternoon I did play with the I value.
Thanks
Shawn
Should that not be GRoll := GRoll + Gx ?
Your ARoll value looks fine - You're computing a number from -180,000 to +180,000, and that should be ok. The GRoll value might be the issue though.
You're taking GX * 4000 / 14375, which means that if GX is < 4, you won't see any result at all. You may be getting a lot of lost precision by doing the divide before you do the accumulation. If I were you, I'd simply be computing a running sum of the Gx values, and then making *that* human readable by doing the scaling. You won't lose that precision forever.
I ran through your other math and it looks to all be correct.
*edit* - Never mind - I see where that Roll value is calculated, but it still means you're losing precision by throwing out the lower few bits of Gx.
Thanks for the reply. My flight loop is 250Hz which is 4 ms. The 4000 is the DT value for calculating my angle from the gyro. How can I get but without using the 4ms. My thought process on the 4000 was to make everything an integer. I think the equation for determining an angle from the ITG3200 is..............
Gx = (Gx * Dt) / 14.375 LSBs
Roll = Roll + Gx
I think I am still not understanding something.
What did you think about the PID loop? It may not have been commented enough to understand.
Thanks
Shawn
3 * 4000 = 12000
12000 / 14375 is a fraction and returns 0
Still though how can I get by with out using the 4000 and calculate an accurate angle? I am missing something with accumulating the gyro reading I think.
Shawn
Is the same thing happening in my PID loop? I am dividing the PID output by 10000 to try and scale down the output to my motors but anything less than 10000 will be lost. Hmmm the old brain matter is a churning but I don't quite have it.
Shawn