If I had a nickel for every time I thought someone else's code was messed up, but then it turned out to be my fault, well... I'm not sure they've made that many nickels.
Glad you got it sorted out. Don't get discouraged - this stuff isn't simple, and everyone who's been through it before you probably had similar issues.
Thanks guys that makes me feel better. I put together a very simple program to read the ADXL345 and turn the X and Y axis values into degrees(attached as the Zip file). For simplicity of displaying with PST I took the X and Y values times 100 and converted them to integers. I put this together so you guys could take a look at it and critique it or whatnot. I think it is complete in its simplest form so maybe someone else can learn step by step.
Next I am going to put together a gyro to angle program and post that also. I will follow that up with a simple complimentary filter program. From there I am not sure where I will go. Like I said it will either be Direct Cosine Matrix(DCM) or PID loops.
I am not sure that I will ever get to the Kalman Filter. Does it matter if I just keep my questions rolling in this thread? Very soon, the questions may not be related to complimentary filters or kalman filters, I hope.
I did find a way to calculate the angle of an axis on a ACC using all 3 axis's and the ATAN function. The results were kind of interesting, The ATAN function if I am not mistaking ranges from 90 to -90 degrees. Which seems kind of limiting, but maybe not if a guy is not going to try to do flips. I am not sure you can even do flips with a quad when the ACC is turned on. If I do flips with my guad I run in acro mode(gyros only). Also there are a lot more math steps involved. The web page I read said that using 3 axis's is the most accurate way of reading the angle of any one axis on an ACC. This is the web site,http://www.hobbytronics.co.uk/accelerometer-info, look for Measuring Tilt Angle using Three Axis about 3/4 of the way down the page.
Here is a simple program to read the X and Y axis from a Gyro and convert to an angle. Keep in mind that since it is a gyro there will be gyro drift, could be 1 to 2 degrees a minute. Like before do whatever with it you want. Use it, learn from it, critique it, help me learn from it, whatever. Oh ya, I am using the 6DOF from sparkfun and the Gyro is an ITG3200 I2C mems gyro.
Next I will put together a simple complimentary filter program and post it on the forum.
Jason,
For your DCM quad are you still using the IntPID-Scaled from the hoverflysport project you made. I have been reading about PID's for the last couple days. Getting ready to try one.
Shawn
Here is a simple complimentary filter. It is basically a combination of the programs from posts #34 and #33. The ACC does not have a zeroing routine but one could be added fairly easy. The program is short and sweet and hopefully commented well enough for people to get the gist of how to put together a simple complimentary filter. I haven't tested it yet, but I am pretty sure that if the board is rotated more than 180 degrees there will be problems. The gyro angle calculations will just keep adding forever I think. At this point in time I am not really interested in anything over 90 degrees. The board functions from 0 to 180 degrees and 0 to -180 degrees.
Yeah, I'm still using the integer PID routines from the QuadX / Sport code. Nearly everything I do uses fixed-point math since floats on the Prop are quite slow by comparison.
hi shawna,
I am watching your development. you are going well.
I have worked complemetery filter based imu. And I have basic flight stabilisation on fixedwing platform with it.
you may want to add lowpass filtering to accelerometer readings
A.ESER
Thanks for the reply, this has been a learning process to say the least. The filtering aspect is kind of why I started this thread. I actually thought that the Complimentary filter was a low pass filter for the ACC and a high pass filter for the gyro. I am not sure how to make a low or high pass filter. Would it be possible for you to explain briefly or give a short example or a link? I have googled low pass and high pass filters in software and did not really understand. Like I said I thought the Complimentary filter did this.
I have seen what they called a first order and second order complimentary filter but I did not explore them very far. I assumed the one I wrote was a first order Complimentary Filter, but I am not sure how to expand on that.
Thanks
Shawn
@Shawn
First of all, I want to ask ? is this a learning only effort? Are you planning to use your imu for controling multicopter or fixedwing aircraft. If its a learning only study, you have a working imu
If you want to use it for flight control, you need extra tricks. You can start with shaking your board on a level surface, observe how much pitch and roll angles affected.
Lowpass filtering for accelerometers mentioned before, is for eliminating vibration effects. It should before, complimentary filtering.
The simplest low-pass filter is "average the last few readings". Low-pass means that it only allows low frequency information to pass through, which means information that's changing slowly. A high-pass filter is one that only allows high frequency information through, or information that's changing rapidly.
This code from Wikipedia implements a proper low-pass filter:
// Return RC low-pass filter output samples, given input samples,
// time interval dt, and time constant RC
function lowpass(real[0..n] x, real dt, real RC)
var real[0..n] y
var real alpha := dt / (RC + dt)
y[0] := x[0]
for i from 1 to n
y[i] := alpha * x[i] + (1.0-alpha) * y[i-1]
return y
RC is the "filter constant" and dt is "delta time", or time between sample measurements.
The code above does the work on a block of samples you've already taken, stored in an array, but it's quite easy to change the code to do it as new samples arrive.
Look at that code, and run it with values of Alpha from 0.0 to 1.0, and pay attention to how it behaves. 1.0 means "everything gets through the filter" (no filtering at all), but 0.0 means "nothing gets through the filter". Setting the Alpha value between those two extremes allows you to tune how responsive your output will be to changing inputs. Plug that in to your accelerometer test program and watch the numbers with different filtering values. It'd be worth capturing a bunch of them and graphing the results to see how different filter strengths affect the readings.
Jason and A.ESER,
The last two posts you guys put up has really got my brain a running, thanks.
I want to build a quadcopter that will maintain attitude, altitude and also heading, I am not sure if heading is part of the attitude definition. At the present time I want to do this to take pictures. In the future if I can make this work I may play with way points or FPV, but one step at a time. I waste a lot of time playing with this stuff and sometimes my family gets frustrated with me, so I would like to think I am doing more than playing, I would like to think I am learning.
Ok first question and I will have a few, but I have to formulate all of them in my head. If this equation that A. ESER posted is a low pass filter.
i_Ax := f32.FAdd(f32.FMul(i_Ax,0.99),f32.FMul(f32.FFloat(A x),0.01))
Then would doing this to it make a high pass filter? I realize that one would not do this for an ACC.
Ok for now that is all I can think of but there will be more. Eventually I would like to get to Jason's DCM code. There is no doubt in my mind that the DCM code will out perform the Complimentary Filter. I am going to see how the Complimentary Filter works on my quad first. I have read through the DCM code a couple of time and get pretty lost. There is some spin syntax that still gives me troubles so I need to figure that out before I can understand Jason's DCM code. Then the small task of refreshing myself with matrices. I remember doing them back in the day and understanding them but not anymore.
This question kind of reflects on my first post in this thread. Using the code Jason posted in post #41. If I have a Sample Rate of 250 Hz and an Alpha of .5 does this mean I have a 125 Hz low pass filter? Or if I have the same Sample Rate but change my Alpha to .3, would this be a 75 Hz low pass filter?
SampleRate * Alpha = HzLowPassFilter
There's a value called RC, which represents resistance and capacitance in an electronic filter, that is used to derive the Alpha value, like this:
Alpha := dt/(RC+dt)
According to Wikipedia, the cutoff frequency of the filter can be computed using:
Freq := 1 / (2.0 * PI * RC)
That said, it's not quite as simple as you make it out. The low-pass filter doesn't just cut frequencies above a certain value. They begin to diminish above the cutoff frequency, and the higher the frequency the more it'll be diminished.
It's also not necessary to completely understand all this to be able to use it. Start with an alpha value of 1.0, and slowly reduce it until your numbers are responsive enough by also stable enough to be useful. There's probably a sweet spot.
It's also worth mentioning that physical vibration damping can be quite important in a quad - Using rubber grommets or foam pads to isolate the gyro / accelerometer from the frame will go a long way.
Thanks for the help guys. I think it is time to start putting all of the pieces together. I ordered some proto-boards from parallax and I am going to build a frame to hold my quad in. I will have more question I am sure, but I am ready to start playing. The DCM will work much better I am sure but there is nothing wrong with trying the complimentary filter first. I suspect that the quad will fly, maybe better than what I think but that is yet to be seen. I am going to start a build thread for my project, and when I get to different parts in it I will link back to my side posts like this one and other ones. I am sure I will have to start some more threads when I have more questions. I have started dabbling in the PID loops and although I do not fully understand them, I am making progress. I think, Ha. I will probably start a thread for that one also.
Thanks Again
Shawn
If you've seen any of the videos for the QuadX or Propeller with Propellers project, neither of them use anything but a gyro. You can get remarkably stable flight from very simple code. Self-leveling is a bit harder, then adding heading is harder still. Once you get to altitude control and waypoints things get more complicated again. Each level adds complexity, so you're doing it right by starting simple and working your way up.
Hey guys its been awhile since I last posted. I have still been working on the quad project. Mainly doing some thinking and learning. I have a flight control program almost written. The code is pretty bloated and I think I am only going to get about 200Hz out of it but it is a place to start. Also I have killed all of my cogs.
My question is, can I filter the output of my complimentary filter with the filter Jason posted in post # 41?
I am rounding my angles off to 2 decimal places and I know A.ESER is going to blast me for that but I think the resolution may not even be that good. When my board is setting on my table the readings for roll and pitch (x and Y axis) bounce from .01 to .12 . It seems to be pretty noisy sitting still on the table. I am calculating angles for my gyro then I am running my zeroed acc readings through a low pass digital filter. Then I am calculating my ACC angles and finally mixing the ACC and GYRO angles together using a complimentary filter.
Thanks
Shawn
Correct me if I am wrong but I do not think I need a separate digital low pass filter for my acc when I mix my acc and gyro with a complimentary filter. I have bypassed the digital filter for my acc and set my complimentary filter to .995 factor for my gyro and .005 for my acc. I changed these values until I had no more drift while my board is sitting on my table. My values are now bouncing slowly in between .01 and .09 degrees and I seem to get pretty good response when I tilt my board.
Shawn
I have used additional lowpass filter for 500Hz IMU loop. If your frequency is 200hz and complimentery filter factors .995 gyro and .005 acc., you may not need for additional lowpass.
Presicion of pitch & roll angles not so important for flight control, For multicopters, I think most important factor is quick response imu and control loop. I strongly suggest reading
code of MultiWiii project. They are using a complementery filter based imu.
By the way, I have tested DCM code on a fixedwing platform. Results are not good, DCM code strongly affected by centrifugal accelerations in turns, even with a acceleration
magnitude vector monitoring. ( Accelereation Magnitude = Sqrt(Ax*Ax + Ay*Ay + Az*Az and if Magnitude > 1.5G and Magnitude < 0.5G I am cancelling accelereometers )
I think, I need gps based centrifugal compansation like Ardupilot project.
But you dont need centrifugal compansation for multicopter control. http://vimeo.com/23909597 This is my tricopter with complimentery filter based imu, last year.
Hey guys,
More questions for ya all. I have not got the quad flying yet but I am making good progress. As I write the code, I have discovered that I am not sure how to mix the yaw command into the pitch and roll. In my quest I have been reading Jason's hoverfly code more and I think I finally figured out the heart of it. I could not figure out how he was calculating angles from the gyro. But I don't think he is, he is just setting a degrees per second rate with the input from the remote and then the PID's are trying to speed the motors up and down to match. At least I think that is what he is doing.
So on my quad I think instead of using an angle for my yaw I will just try to maintain a rate or rotation. While I was reading up on this, I thought of more questions. They are not new questions and you all have brought the issues up before but I am still trying to understand it all.
1)Which device has problems with centripetal force, is it the gyro?
2) Is the centripetal for introduced into the quad by applying yaw?
3)If questions 1 and 2 are correct is the problem with the complimentary sensor the fact that the ACC is basicaly used to get rid of the drift in the gyro? The ACC is high passed so the signal is not mixed in fast enough to help make up with centripetal force.
I have a hard time reading equations, I do not understand most of the symbols and characters in them. That is one of the biggest reasons I have not dove into the DCM code yet. I was reading an interesting site where the yaw from the gyro can be mixed into the complimentary filter to compensate for the centripetal force. It will only work to maybe 45 degrees of roll and pitch, which is what I was shooting for anyways. Has anyone found any complimentary filter examples that include yaw. This is the only one I can find but I have not had time to totally wrap my head around it. If anyone is interested here is the link. I found it to be an interesting article and I am going to try and implement it. I found his code on the internet somewhere but did not get it bookmarked. I don't understand if the yaw value that is calculated into the equation is in the form of an angle or just a rate degrees per second. http://scolton.blogspot.com/2012/09/fun-with-complementary-filter-multiwii.html
Here is the math in the link from my last post for adding yaw compensation.
I am not sure why he does this, INV_RAD = PI/180
qyro_yaw * DT would give an angle.
I think AA is just alpha or a number less than 1.
In that link he graphed some data which looks like it worked but I don't understand why, Is it because the yaw axis on the gyro still read rotation on the z axis when pitch and roll are not 0 degrees, and so by being able to do this it can be apllied to the pitch and roll angles to compensate for centripecal force.
// integrate pitch rate into pitch angle int_pitch = angle_pitch + gyro_pitch * DT; // integrate component of yaw rate into pitch angle int_pitch += angle_roll * INV_RAD * gyro_yaw * DT; // filter with error feedback from pitch accelerometer error_pitch = accel_pitch - int_pitch; angle_pitch = int_pitch + AA * error_pitch; // integrate roll rate into roll angle int_roll = angle_roll + gyro_roll * DT; // integrate component of yaw rate into roll angle int_roll -= angle_pitch * INV_RAD * gyro_yaw * DT; // filter with error feedback from roll accelerometer error_roll = accel_roll - int_roll; angle_roll = int_roll + AA * error_roll;
Been doing some more reading and thinking. Correct me if I am wrong but as far as I can see, the ACC main purpose is to get rid of gyro drift. Actually in my mind it is its only function. If the gyro had no drift one could maintain attitude with just a gyro, correct me if I am wrong on that please.
So I came up with the idea of turning the ACC off or scaling it down as I applied rudder (yaw). Then I decided to look at the multiwii code, I think that they are doing that in the multiWII software. I am not real sure of that because I have a hard time of reading and understanding the code. I think that the ardrino is a modified form of C++, at least that is what I have been led to believe.
So this led me to the thought that I am wasting way to much processor time computing angles from the ACC. An estimation should be good enough, I think! I am going to explore the option of using a cordic table to get the angle of my ACC, I hope this is accurate enough. I am using Jason's ITG3200 code and it looks like that program actually computes the angle of the ACC with a cordic table but when I read the values I get a funky numbers. I am not sure how to use that part of the program.
So more questions, some are repeats. These ideas I have are based on some assumptions that I am not sure that I am right on.
1) Does centripetal force affect GYRO readings or just ACC readings?
2) Is the ACC main function to get rid of GYRO drift which results in the ability to maintain attitude?
3) Is a Cordic table accurate enough to use in a quadcopter and is it faster than using floating point math?
4) Not really related but at the top of my mind, DCM is not capable of altitude hold, just attitude right?
5) Does centripetal force affect a DCM model also?
I do not understand cordic tables at all, I undertand the basics, I think they are basically a look up table. To me they just look like a group of messy numbers. I have done some reading to try and figure out how to use one but I did not understand the examples I found. I will have to do some more digging. At this point I am going to maintain coarse and use the floating point math but if I get acceptable results I am going to try and switch over to integers to try and speed up the math.
The gyro only quad seems in my mind that it is way easier to implement than even a complimentary filter. I may end up having to take a step back to that and work up but at this point that is not my plan. My plan is to get the quad running with the complimentary filter with no yaw input. Then add yaw and see what I can crash the quad into.
I don't remember off the top of my head what the range of numbers out of the CORDIC routine is, but I'm betting it's integer, and a power of two value will be a full circle - probably 0 to 65535 (16 bits) or something around there.
1) Just accelerometer readings
2) Yes, and to give a frame of reference. If the quad is on an angle when you turn it on, the accelerometer can tell you that. The gyro can't.
3) Yes. CORDIC can be written to give as many bits of precision as you need. I don't recall how many I'm returning.
4) Correct. DCM is simply orientation. Altitude is yet another thing.
5) Yes.
Without yaw control you're going to have a hard time controlling the thing. Remember that when you tell the quad to go left, it goes toward IT's left, not yours. With no yaw handling in the control loop, the quad will be free to spin, making it VERY hard for you to tell which way is the right way to direct it if necessary. Just a heads up.
Ok I have decided to switch everything over to Integers for the following reasons.
1) I believe that adequate precision can be maintained.
2) Seems to be allot faster.
3) Saves two Cogs from the Float 32 routine.
4) Saves a ton of memory.
5) I am kind of bias but I do not really see any negatives at this point.
6) I was using both Integers and FP's kind of inefficient.
Here is my test code to read the Cordic angle from Jason's ITG3200 object. I would like you guys to look at it and critique it for me, help me stream line it if you would. I still do not understand the power of 2 statement Jason mention earlier. I may understand the theory but not the terminology if that statement makes sense. I got Jason's code to work but I do not understand what Jason meant by power of 2.
CON _clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
OBJ
AccGyro : "ITG 3200 Sport pasm" 'By Jason Dorie
PST : "Parallax Serial Terminal" 'By Jeff Martin, Andy Lindsay, Chip Gracey
VAR
Long Ax,Low,High 'ACC readings from ADXL345
PUB Main
''''''Initializes a Couple of Things''''''
PST.Start(1_000_000) 'Sets BaudRate for PST.
PST.Clear 'Clears PST Screen.
AccGyro.Start(6,7) 'Starts Gyro and ACC(6DOF). Sets I2C Pins.
Repeat
''''''''''''Takes 68.6 uS
Ax := AccGyro.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
Ax := Ax / 1000 'I want 3 decimal places of resolution.
If Ax > 180000 'I do not want 0 to 360 I want 0 to 180 and 0 to -180.
Ax := Ax - 360000
''''''''''''''''''''''''''''
PST.Clear
PST.Dec(Ax) 'Displays angle in pst. 180000 in PST would be 180.000 degrees.
waitcnt(80_000_000 / 10+ cnt )
The ranges of binary numbers are all powers of two, meaning "two to the power of X", where X is a number. A single byte can represent 8 bits of information, or two to the power of 8 (256) unique values, usually from 0 to 255. 16 bits can represent two to the power of 16 unique values, or 65536, usually from 0 to 65535. I say "usually" because it depends on whether the numbers are signed or unsigned. A signed byte can represent -128 to +127. It's still 256 unique values, but the range shifts.
In my code, 0 = 0 degrees, 32768 = 180 degrees, and 65536 = 360 degrees, which is a full rotation, so you can generally treat 0 and 360 (or 0 and 65536) as "the same". You only care which way the quad is pointing, and a heading of 0 is the same as a heading of 360.
In the code that actually uses these numbers, you're better off using the 0 to 65535 range instead of the "scaled into human readable" range. I'm assuming you scaled them into something closer to real degrees just so you could read them.
The only real optimization I can see for your code at this point is this:
Ax := (Ax * 5493) / 1000 'ITG OBJ puts out value 0 to 65535 which is 0 to 360 degrees. 360/65535 =.00549324
Doing that in a single line will run quicker. Or, slightly faster:
Ax := (AccGyro.GetARX * 5493) / 1000 'ITG OBJ puts out value 0 to 65535 which is 0 to 360 degrees. 360/65535 =.00549324
In my quest I have been reading Jason's hoverfly code more and I think I finally figured out the heart of it. I could not figure out how he was calculating angles from the gyro. But I don't think he is, he is just setting a degrees per second rate with the input from the remote and then the PID's are trying to speed the motors up and down to match.
Exactly right - I'm using the information from the gyro to tell me how fast the quad is rotating around a given axis. Then I'm looking at the remote control input to tell me how fast the user WANTS to be rotating around that axis. The difference in those two numbers is what the PID loops are dealing with. That's a subtle simplification - I'm actually taking the SUM of the gyro inputs and the SUM of the user inputs over time, which tells me "this is what angle I think I'm at" and "this is what angle the user wants me at". Those values are fed to the PIDs as the measured value and the set point.
1)Which device has problems with centripetal force, is it the gyro?
2) Is the centripetal for introduced into the quad by applying yaw?
3)If questions 1 and 2 are correct is the problem with the complimentary sensor the fact that the ACC is basicaly used to get rid of the drift in the gyro? The ACC is high passed so the signal is not mixed in fast enough to help make up with centripetal force.
1) Nope - the accelerometer is affected, not the gyro. Think of a car going around a corner at high speed - The centripetal force is what slides you into the door. It "feels like" gravity, which is why it messes up the accelerometer - it can't tell them apart. Even sitting stationary in a car and then flooring it produces a lateral force - the force that presses you into the seat - that an accelerometer would measure, and that too will confuse code that assumes the accelerometer is only reading "gravity". It isn't. It's reading all inertial forces being applied to it.
2) No, it's any inertial force. So, going around a corner at high speed, accelerating or braking hard, etc.
3) The problem with the accelerometer is that it's really noisy, and hellishly unreliable because it's affected by forces other than gravity. So you low-pass filter it to keep it from influencing the gyro readings much. It's mostly just to help stabilize the gyro over time.
Regarding yaw, there's no way to use an accelerometer to deal with yaw. The accelerometer can help with "down" because it reads gravity, which always pulls down. The only way to stabilize yaw is with a gyro, OR using something like a compass (magnetometer). You need a frame of reference. The accelerometer measures gravity, and that's your frame of reference for "down". You need something similar for "north", or just use the gyro Z axis to help keep it mostly stable and not worry about it. (That's what I did).
Jason, thanks for the reply, I understand the power of 2 on the binary, I am just not use to hearing that I guess. There are 2 reasons why I converted the acc angle to degrees. First reason is for readability, the second is because that is what my gyro equation for angle converts to. Also I am converting my rc inputs to degrees. Why is it better to leave the acc angle in the form of 0 to 65535? Is that to maintain a better resolution, or just to save some processor clock cycles.
If everything agrees on the same unit, it doesn't matter what the unit is. In my case I try to leave things in a form that's easier for the machine to work with than for me. In the angle case, I can truncate an angle to be 0 to 359.9999 degrees by just chopping off anything higher than 15 bits. (IE, using AND $FFFF). Limiting a rotation to that range if using your number scheme would require a modulo operation, which is a bit slower.
That said, don't worry too much about it - as long as everything agrees on what the unit is, just work with that. If you can read your numbers reasonably easily, it's probably better to leave them as is, because debugging later will be easier for you.
This is frying my brain! Every time I get to the point where I think I am ready to put it all together I hit another obstacle.
Thanks for the input Jason. I knew the acc was sent through a low pass filter, I miss spoke in the post above, I wouldn't want you to think all of your time has been wasted to this point if I can't even grasp that. ( :
I have everything switched over to integers at this point and the complimentary filter is working for the X and Y axis, but there is a huge problem, and I think it is related to what you have been preaching this whole thread to me.
If I am using just my gyro to produce angle the problem shows it self right away, and I am not talking about gyro drift the acc and comp filter takes care of that. If my board is sitting flat, and I Pitch forward 45 degrees everything reads fine. Now if I yaw 90 degrees from there, the headaches begin. Once the board is yawed 90 degrees the Pitch angle should now be 0 and the roll angle should be 45 degrees but it is not, and I finally understand that now. How does one compensate for that, beats me?
I found the equation in post 51 of this thread and I thought maybe I could use this to fix this problem, but I cannot get it to work.
Some how I believe that the yaw angle needs to be used when calculating the Pitch and Roll angles from the GYRO.
The example in post 51 is the only one I can find that covers this.
Also there is going to be drift on the yaw axis of the gyro, I think the only way to fix this is to run it through a complimentary filter with either a compass or gps.
I know what is going to be said next, try the DCM. I have been studying that also and still do not understand it. I think I understand it more now then the first time I read through it, but not even close enough to implement it.
Jason are you using a compass to stop gyro drift in your DCM version? Also don't you still have to use a complimentary filter to fuse the gyro and acc info together in your DCM?
Thanks
Shawn
Maybe the complimentary filter will take care of the qyro yaw problem with out adding the yaw to the gyros pitch and roll calculations. I don't know! When I rotate the board around with the complimentary filter, the angles seem to look ok. I am just going to have get something flying to test. Just out of curiousity 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.
Shawn
Comments
Glad you got it sorted out. Don't get discouraged - this stuff isn't simple, and everyone who's been through it before you probably had similar issues.
Next I am going to put together a gyro to angle program and post that also. I will follow that up with a simple complimentary filter program. From there I am not sure where I will go. Like I said it will either be Direct Cosine Matrix(DCM) or PID loops.
I am not sure that I will ever get to the Kalman Filter. Does it matter if I just keep my questions rolling in this thread? Very soon, the questions may not be related to complimentary filters or kalman filters, I hope.
I did find a way to calculate the angle of an axis on a ACC using all 3 axis's and the ATAN function. The results were kind of interesting, The ATAN function if I am not mistaking ranges from 90 to -90 degrees. Which seems kind of limiting, but maybe not if a guy is not going to try to do flips. I am not sure you can even do flips with a quad when the ACC is turned on. If I do flips with my guad I run in acro mode(gyros only). Also there are a lot more math steps involved. The web page I read said that using 3 axis's is the most accurate way of reading the angle of any one axis on an ACC. This is the web site,http://www.hobbytronics.co.uk/accelerometer-info, look for Measuring Tilt Angle using Three Axis about 3/4 of the way down the page.
ACC Angle Reading FP32 V2.zip
Shawn
Next I will put together a simple complimentary filter program and post it on the forum.
Gyro Angle Reading FP32.zip
Shawn
For your DCM quad are you still using the IntPID-Scaled from the hoverflysport project you made. I have been reading about PID's for the last couple days. Getting ready to try one.
Shawn
Complimentary_X_Y_ANGLE_6DOF.zip
I am watching your development. you are going well.
I have worked complemetery filter based imu. And I have basic flight stabilisation on fixedwing platform with it.
you may want to add lowpass filtering to accelerometer readings
Thanks for the reply, this has been a learning process to say the least. The filtering aspect is kind of why I started this thread. I actually thought that the Complimentary filter was a low pass filter for the ACC and a high pass filter for the gyro. I am not sure how to make a low or high pass filter. Would it be possible for you to explain briefly or give a short example or a link? I have googled low pass and high pass filters in software and did not really understand. Like I said I thought the Complimentary filter did this.
I have seen what they called a first order and second order complimentary filter but I did not explore them very far. I assumed the one I wrote was a first order Complimentary Filter, but I am not sure how to expand on that.
Thanks
Shawn
First of all, I want to ask ? is this a learning only effort? Are you planning to use your imu for controling multicopter or fixedwing aircraft. If its a learning only study, you have a working imu
If you want to use it for flight control, you need extra tricks. You can start with shaking your board on a level surface, observe how much pitch and roll angles affected.
Lowpass filtering for accelerometers mentioned before, is for eliminating vibration effects. It should before, complimentary filtering.
'Read & Filter Accelerometers ...
Ax := Sensor.GetAX
Ay := Sensor.GetAY
Az := Sensor.GetAZ
i_Ax := f32.FAdd(f32.FMul(i_Ax,0.99),f32.FMul(f32.FFloat(Ax),0.01))
i_Ay := f32.FAdd(f32.FMul(i_Ay,0.99),f32.FMul(f32.FFloat(Ay),0.01))
i_Az := f32.FAdd(f32.FMul(i_Az,0.99),f32.FMul(f32.FFloat(Az),0.01))
My imu loop frequency is 500 hz, you can start with lover values, too much filtering causes latencies.
By the way, Jason's DCM code is performing much better than complimentary filter. I am trying to test it on fixedwing aircraft these days.
Aydin
This code from Wikipedia implements a proper low-pass filter:
// Return RC low-pass filter output samples, given input samples, // time interval dt, and time constant RC function lowpass(real[0..n] x, real dt, real RC) var real[0..n] y var real alpha := dt / (RC + dt) y[0] := x[0] for i from 1 to n y[i] := alpha * x[i] + (1.0-alpha) * y[i-1] return yRC is the "filter constant" and dt is "delta time", or time between sample measurements.
The code above does the work on a block of samples you've already taken, stored in an array, but it's quite easy to change the code to do it as new samples arrive.
Look at that code, and run it with values of Alpha from 0.0 to 1.0, and pay attention to how it behaves. 1.0 means "everything gets through the filter" (no filtering at all), but 0.0 means "nothing gets through the filter". Setting the Alpha value between those two extremes allows you to tune how responsive your output will be to changing inputs. Plug that in to your accelerometer test program and watch the numbers with different filtering values. It'd be worth capturing a bunch of them and graphing the results to see how different filter strengths affect the readings.
The last two posts you guys put up has really got my brain a running, thanks.
I want to build a quadcopter that will maintain attitude, altitude and also heading, I am not sure if heading is part of the attitude definition. At the present time I want to do this to take pictures. In the future if I can make this work I may play with way points or FPV, but one step at a time. I waste a lot of time playing with this stuff and sometimes my family gets frustrated with me, so I would like to think I am doing more than playing, I would like to think I am learning.
Ok first question and I will have a few, but I have to formulate all of them in my head. If this equation that A. ESER posted is a low pass filter.
i_Ax := f32.FAdd(f32.FMul(i_Ax,0.99),f32.FMul(f32.FFloat(A x),0.01))
Then would doing this to it make a high pass filter? I realize that one would not do this for an ACC.
i_Ax := f32.FAdd(f32.FMul(i_Ax,0.01),f32.FMul(f32.FFloat(A x),0.99))
Ok for now that is all I can think of but there will be more.
Eventually I would like to get to Jason's DCM code. There is no doubt in my mind that the DCM code will out perform the Complimentary Filter. I am going to see how the Complimentary Filter works on my quad first. I have read through the DCM code a couple of time and get pretty lost. There is some spin syntax that still gives me troubles so I need to figure that out before I can understand Jason's DCM code. Then the small task of refreshing myself with matrices. I remember doing them back in the day and understanding them but not anymore.
SampleRate * Alpha = HzLowPassFilter
Alpha := dt/(RC+dt)
According to Wikipedia, the cutoff frequency of the filter can be computed using:
Freq := 1 / (2.0 * PI * RC)
That said, it's not quite as simple as you make it out. The low-pass filter doesn't just cut frequencies above a certain value. They begin to diminish above the cutoff frequency, and the higher the frequency the more it'll be diminished.
It's also not necessary to completely understand all this to be able to use it. Start with an alpha value of 1.0, and slowly reduce it until your numbers are responsive enough by also stable enough to be useful. There's probably a sweet spot.
It's also worth mentioning that physical vibration damping can be quite important in a quad - Using rubber grommets or foam pads to isolate the gyro / accelerometer from the frame will go a long way.
Thanks Again
Shawn
My question is, can I filter the output of my complimentary filter with the filter Jason posted in post # 41?
I am rounding my angles off to 2 decimal places and I know A.ESER is going to blast me for that but I think the resolution may not even be that good. When my board is setting on my table the readings for roll and pitch (x and Y axis) bounce from .01 to .12 . It seems to be pretty noisy sitting still on the table. I am calculating angles for my gyro then I am running my zeroed acc readings through a low pass digital filter. Then I am calculating my ACC angles and finally mixing the ACC and GYRO angles together using a complimentary filter.
Thanks
Shawn
Shawn
I have used additional lowpass filter for 500Hz IMU loop. If your frequency is 200hz and complimentery filter factors .995 gyro and .005 acc., you may not need for additional lowpass.
Presicion of pitch & roll angles not so important for flight control, For multicopters, I think most important factor is quick response imu and control loop. I strongly suggest reading
code of MultiWiii project. They are using a complementery filter based imu.
By the way, I have tested DCM code on a fixedwing platform. Results are not good, DCM code strongly affected by centrifugal accelerations in turns, even with a acceleration
magnitude vector monitoring. ( Accelereation Magnitude = Sqrt(Ax*Ax + Ay*Ay + Az*Az and if Magnitude > 1.5G and Magnitude < 0.5G I am cancelling accelereometers )
I think, I need gps based centrifugal compansation like Ardupilot project.
But you dont need centrifugal compansation for multicopter control.
http://vimeo.com/23909597 This is my tricopter with complimentery filter based imu, last year.
More questions for ya all. I have not got the quad flying yet but I am making good progress. As I write the code, I have discovered that I am not sure how to mix the yaw command into the pitch and roll. In my quest I have been reading Jason's hoverfly code more and I think I finally figured out the heart of it. I could not figure out how he was calculating angles from the gyro. But I don't think he is, he is just setting a degrees per second rate with the input from the remote and then the PID's are trying to speed the motors up and down to match. At least I think that is what he is doing.
So on my quad I think instead of using an angle for my yaw I will just try to maintain a rate or rotation. While I was reading up on this, I thought of more questions. They are not new questions and you all have brought the issues up before but I am still trying to understand it all.
1)Which device has problems with centripetal force, is it the gyro?
2) Is the centripetal for introduced into the quad by applying yaw?
3)If questions 1 and 2 are correct is the problem with the complimentary sensor the fact that the ACC is basicaly used to get rid of the drift in the gyro? The ACC is high passed so the signal is not mixed in fast enough to help make up with centripetal force.
I have a hard time reading equations, I do not understand most of the symbols and characters in them. That is one of the biggest reasons I have not dove into the DCM code yet. I was reading an interesting site where the yaw from the gyro can be mixed into the complimentary filter to compensate for the centripetal force. It will only work to maybe 45 degrees of roll and pitch, which is what I was shooting for anyways. Has anyone found any complimentary filter examples that include yaw. This is the only one I can find but I have not had time to totally wrap my head around it. If anyone is interested here is the link. I found it to be an interesting article and I am going to try and implement it. I found his code on the internet somewhere but did not get it bookmarked. I don't understand if the yaw value that is calculated into the equation is in the form of an angle or just a rate degrees per second.
http://scolton.blogspot.com/2012/09/fun-with-complementary-filter-multiwii.html
Thanks
Shawn
I am not sure why he does this, INV_RAD = PI/180
qyro_yaw * DT would give an angle.
I think AA is just alpha or a number less than 1.
In that link he graphed some data which looks like it worked but I don't understand why, Is it because the yaw axis on the gyro still read rotation on the z axis when pitch and roll are not 0 degrees, and so by being able to do this it can be apllied to the pitch and roll angles to compensate for centripecal force.
// integrate pitch rate into pitch angle
int_pitch = angle_pitch + gyro_pitch * DT;
// integrate component of yaw rate into pitch angle
int_pitch += angle_roll * INV_RAD * gyro_yaw * DT;
// filter with error feedback from pitch accelerometer
error_pitch = accel_pitch - int_pitch;
angle_pitch = int_pitch + AA * error_pitch;
// integrate roll rate into roll angle
int_roll = angle_roll + gyro_roll * DT;
// integrate component of yaw rate into roll angle
int_roll -= angle_pitch * INV_RAD * gyro_yaw * DT;
// filter with error feedback from roll accelerometer
error_roll = accel_roll - int_roll;
angle_roll = int_roll + AA * error_roll;
I am kind of confused!
Thanks
Shawn
So I came up with the idea of turning the ACC off or scaling it down as I applied rudder (yaw). Then I decided to look at the multiwii code, I think that they are doing that in the multiWII software. I am not real sure of that because I have a hard time of reading and understanding the code. I think that the ardrino is a modified form of C++, at least that is what I have been led to believe.
So this led me to the thought that I am wasting way to much processor time computing angles from the ACC. An estimation should be good enough, I think! I am going to explore the option of using a cordic table to get the angle of my ACC, I hope this is accurate enough. I am using Jason's ITG3200 code and it looks like that program actually computes the angle of the ACC with a cordic table but when I read the values I get a funky numbers. I am not sure how to use that part of the program.
So more questions, some are repeats. These ideas I have are based on some assumptions that I am not sure that I am right on.
1) Does centripetal force affect GYRO readings or just ACC readings?
2) Is the ACC main function to get rid of GYRO drift which results in the ability to maintain attitude?
3) Is a Cordic table accurate enough to use in a quadcopter and is it faster than using floating point math?
4) Not really related but at the top of my mind, DCM is not capable of altitude hold, just attitude right?
5) Does centripetal force affect a DCM model also?
I do not understand cordic tables at all, I undertand the basics, I think they are basically a look up table. To me they just look like a group of messy numbers. I have done some reading to try and figure out how to use one but I did not understand the examples I found. I will have to do some more digging. At this point I am going to maintain coarse and use the floating point math but if I get acceptable results I am going to try and switch over to integers to try and speed up the math.
The gyro only quad seems in my mind that it is way easier to implement than even a complimentary filter. I may end up having to take a step back to that and work up but at this point that is not my plan. My plan is to get the quad running with the complimentary filter with no yaw input. Then add yaw and see what I can crash the quad into.
Thanks
Shawn
1) Just accelerometer readings
2) Yes, and to give a frame of reference. If the quad is on an angle when you turn it on, the accelerometer can tell you that. The gyro can't.
3) Yes. CORDIC can be written to give as many bits of precision as you need. I don't recall how many I'm returning.
4) Correct. DCM is simply orientation. Altitude is yet another thing.
5) Yes.
Without yaw control you're going to have a hard time controlling the thing. Remember that when you tell the quad to go left, it goes toward IT's left, not yours. With no yaw handling in the control loop, the quad will be free to spin, making it VERY hard for you to tell which way is the right way to direct it if necessary. Just a heads up.
Thanks
Shawn
1) I believe that adequate precision can be maintained.
2) Seems to be allot faster.
3) Saves two Cogs from the Float 32 routine.
4) Saves a ton of memory.
5) I am kind of bias but I do not really see any negatives at this point.
6) I was using both Integers and FP's kind of inefficient.
Here is my test code to read the Cordic angle from Jason's ITG3200 object. I would like you guys to look at it and critique it for me, help me stream line it if you would. I still do not understand the power of 2 statement Jason mention earlier. I may understand the theory but not the terminology if that statement makes sense. I got Jason's code to work but I do not understand what Jason meant by power of 2.
CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 OBJ AccGyro : "ITG 3200 Sport pasm" 'By Jason Dorie PST : "Parallax Serial Terminal" 'By Jeff Martin, Andy Lindsay, Chip Gracey VAR Long Ax,Low,High 'ACC readings from ADXL345 PUB Main ''''''Initializes a Couple of Things'''''' PST.Start(1_000_000) 'Sets BaudRate for PST. PST.Clear 'Clears PST Screen. AccGyro.Start(6,7) 'Starts Gyro and ACC(6DOF). Sets I2C Pins. Repeat ''''''''''''Takes 68.6 uS Ax := AccGyro.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 Ax := Ax / 1000 'I want 3 decimal places of resolution. If Ax > 180000 'I do not want 0 to 360 I want 0 to 180 and 0 to -180. Ax := Ax - 360000 '''''''''''''''''''''''''''' PST.Clear PST.Dec(Ax) 'Displays angle in pst. 180000 in PST would be 180.000 degrees. waitcnt(80_000_000 / 10+ cnt )ThanksShawn
In my code, 0 = 0 degrees, 32768 = 180 degrees, and 65536 = 360 degrees, which is a full rotation, so you can generally treat 0 and 360 (or 0 and 65536) as "the same". You only care which way the quad is pointing, and a heading of 0 is the same as a heading of 360.
In the code that actually uses these numbers, you're better off using the 0 to 65535 range instead of the "scaled into human readable" range. I'm assuming you scaled them into something closer to real degrees just so you could read them.
The only real optimization I can see for your code at this point is this: Doing that in a single line will run quicker. Or, slightly faster: J
Exactly right - I'm using the information from the gyro to tell me how fast the quad is rotating around a given axis. Then I'm looking at the remote control input to tell me how fast the user WANTS to be rotating around that axis. The difference in those two numbers is what the PID loops are dealing with. That's a subtle simplification - I'm actually taking the SUM of the gyro inputs and the SUM of the user inputs over time, which tells me "this is what angle I think I'm at" and "this is what angle the user wants me at". Those values are fed to the PIDs as the measured value and the set point.
1) Nope - the accelerometer is affected, not the gyro. Think of a car going around a corner at high speed - The centripetal force is what slides you into the door. It "feels like" gravity, which is why it messes up the accelerometer - it can't tell them apart. Even sitting stationary in a car and then flooring it produces a lateral force - the force that presses you into the seat - that an accelerometer would measure, and that too will confuse code that assumes the accelerometer is only reading "gravity". It isn't. It's reading all inertial forces being applied to it.
2) No, it's any inertial force. So, going around a corner at high speed, accelerating or braking hard, etc.
3) The problem with the accelerometer is that it's really noisy, and hellishly unreliable because it's affected by forces other than gravity. So you low-pass filter it to keep it from influencing the gyro readings much. It's mostly just to help stabilize the gyro over time.
Regarding yaw, there's no way to use an accelerometer to deal with yaw. The accelerometer can help with "down" because it reads gravity, which always pulls down. The only way to stabilize yaw is with a gyro, OR using something like a compass (magnetometer). You need a frame of reference. The accelerometer measures gravity, and that's your frame of reference for "down". You need something similar for "north", or just use the gyro Z axis to help keep it mostly stable and not worry about it. (That's what I did).
J
Thanks
Shawn
That said, don't worry too much about it - as long as everything agrees on what the unit is, just work with that. If you can read your numbers reasonably easily, it's probably better to leave them as is, because debugging later will be easier for you.
Thanks for the input Jason. I knew the acc was sent through a low pass filter, I miss spoke in the post above, I wouldn't want you to think all of your time has been wasted to this point if I can't even grasp that. ( :
I have everything switched over to integers at this point and the complimentary filter is working for the X and Y axis, but there is a huge problem, and I think it is related to what you have been preaching this whole thread to me.
If I am using just my gyro to produce angle the problem shows it self right away, and I am not talking about gyro drift the acc and comp filter takes care of that. If my board is sitting flat, and I Pitch forward 45 degrees everything reads fine. Now if I yaw 90 degrees from there, the headaches begin. Once the board is yawed 90 degrees the Pitch angle should now be 0 and the roll angle should be 45 degrees but it is not, and I finally understand that now. How does one compensate for that, beats me?
I found the equation in post 51 of this thread and I thought maybe I could use this to fix this problem, but I cannot get it to work.
Some how I believe that the yaw angle needs to be used when calculating the Pitch and Roll angles from the GYRO.
The example in post 51 is the only one I can find that covers this.
Also there is going to be drift on the yaw axis of the gyro, I think the only way to fix this is to run it through a complimentary filter with either a compass or gps.
I know what is going to be said next, try the DCM. I have been studying that also and still do not understand it. I think I understand it more now then the first time I read through it, but not even close enough to implement it.
Jason are you using a compass to stop gyro drift in your DCM version? Also don't you still have to use a complimentary filter to fuse the gyro and acc info together in your DCM?
Thanks
Shawn
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.
Shawn