MPU6050 Pitch & Roll with Complimentary Filter on Raspberry Pi Pico W

[00:00:00]hello guys this is Paul mcarter with toptechboy dcom and we're here today with episode number 51 in our incredible new tutorial Series where you are unleashing the power of your raspberry piie Pico W what I will need you to do is pour yourself a nice tall glass of ice cold coffee that would be straight up black coffee poured over ice no sugar no sweeteners none needed and as you're pouring your coffee as always I want to give a shout out to our friends over at sunfounder sunfounder is actually sponsoring this most excellent series of video lessons and in this class we will be using the Kepler kit for Raspberry Pi Pico W now most of you guys probably already have your gear but if you don't look down in the description there is a link over to Amazon and you can hop on over there and pick your kid up and believe me your life and my life are going to be a whole lot easier if we are working on identical Hardware but enough of this Shameless self-promotion let's jump in and talk about what I'm going to teach you today what I'm going to do is I'm going to show you my solution to the homework assignment that I gave in lesson number 50 but I must first ask how many of you guys were successful if you were successful comment down below I Am Legend double chess bump and if you were not successful leave a comment down below I folded up like a cheap Walmart lawn chair okay hopefully you guys got this because really it was just pulling together the stuff that we've learned in the last lessons and what the homework assignment was was to take the work that we did last week where we were getting the kind of ultimate measurements from the mpu6050 right we were measuring acceleration we were measuring from the Gyros gyroscopes and then we were fusing the data together to get something that was fast it was accurate and it was low noise and then we added the kind of cherry on the top of the ice cream by also pushing out those long term steady state drift terms so we have something now that is really quite possibly about the most accurate measurements that you can get from the mpu6050 for for roll uh for uh roll and Pitch okay so we got all that but now what we want to do is we want to make a useful little widget a useful little Gizmo and so what your homework was was to actually I should probably get out of your way was to actually come in now and use that algorithm that we developed for the mpu6050 but then to connect the uh the OLED 1306 and to have a useful little Gizmo where you have like a reticle and then you have a bubble and that bubble moves around indicating how far off you were from Level so it would be kind of like a 2d Carpenters level so you could see am I level in the pitch direction am I level in the roll Direction now as you all know with the Carpenters level you're just eyeballing a bubble and that's good for qualitative you know indication of level and we do have that we have a little bubble moving around but we also want a quantitative we want a quantitative indication so we also want to actually print out on the little screen roll and Pitch okay so way too much introductory talking let's jump in and let's see if we can get this thing done I don't want to start all the way from scratch and you remember a few lessons ago we actually did this with the OLED and we did it just doing sort of a simple measurement of roll and Pitch without putting all the bells and whistles in to get rid of noise and get rid of acceleration errors and and drift errors and all that sort of stuff but it did sort of run the OLED and so I think that would be a good place to start so I will need you to go to the most excellent www.top techboy tocom I need you to use this happy little search icon and search on something like 2x tilt meter from the mpu6050 and you can see here this is our schematic of how we hooked it together we've got the mpu 6050 and the OLED 1306 connected to the Raspberry Pi Pico W and then this was the code that we had developed let's see I'm not sure that copied it so I'm going to copy it the oldfashioned way like this right Mouse click copy now we're going to come over to th and let's go ahead and paste it in th and now let's run it and let's come over here and okay it seems to be getting data and I think you can see that all right okay let me go ahead and let me give you the full view here of it and what you can see is I think this is just getting an approximation of tilt and Roll Just from the accelerometers but look at what you look at what we have we have this circle indicating the bubble and when it is right over the reticle that means you are you are completely level in both axises then we're printing out the pitch in degrees and we're printing out the roll in degrees and then as I tilt it what you can see is I tilt it I get a visual in indication qualitatively from the little circle moving around and then I get a quantitative indication from the printouts up there so how cool is that well is this not the perfect solution well what's the problem if I have a little vibration you see how that bubble is interpreting the vibration and the motion it's interpreting vibration and motion as changes in tilt and that would be an error so if this was some on something like a jeep that you're going off road in and wanting to measure what your inclination is as you're driving what would be the problem if you were bumping this would be all over the place or worse if you were doing something like a quadcopter that your approximation of the Tilt and roll the pitch and roll of your quadcopter would be impacted by vibration and certainly there would be a lot of vibration there so we spent what four or five lessons getting rid of all the noise getting rid of all the errors getting rid of all the short-term errors long-term errors all of that now we want to come back here and put that better understanding of pitch and roll into this program that I might say quite nicely is already uh displaying a simple pitch and roll okay so what we're going to do is we're going to come in here and we're just going to jump in one thing I guess I should show you what I'm doing here huh and I yeah I think that'll be okay I think you'll be able to see the code and the live view here at the same time so we'll leave it there well first of all we know that uh on this code we were just getting the pitch and roll from the accelerometer but we know that we're going to need that gyro data so let's go ahead and let's get this the accelerometer measurements and the gyro measurements are very similar so I'm just going to cut and paste those and then here this is going to be X gyro and that is going to be mpu jro X then this is going to be y gyro and this is going to be mpu do gyro y and then this is going to be Z gyro that's the measurement and that is going to be g y r o gyro okay so now I've got those measurements made so that's good okay then let's see here we've got uh this is uh that code that defines the circle that's good all that looks good so now we're just trying to get a better measurement for pitch and roll and so now I've got this pitch here okay and what I want this to do is I want this to be the pitch from the accelerometer because now I'm going to have a pitch from the accelerometer a pitch from the gyro or a pitch from the accelerometer a pitch from the gyro and a roll from the accelerometer and a roll from the gyro also I don't want to add this other variable so I'm just going to go ahead and I'm going to convert remember this returns an answer in radians and I want to work in degrees and so I'm going to add out here this divide by 2 pi and multiply by 360 and now my pitch a and my roll a will be in degrees instead of radians now I can get rid of this code here okay like that so now I have my pitch accelerometer and I have my roll accelerometer and then uh what I'm going to do now is I'm going to come over and I am going to calculate I'm going to calculate a compensated value and that compensated or you know applying a compliment filter is going to get rid of all the noise in pitch a and roll a so I'm just going to call this role compensated where I'm going to do all my magic in one line of code so the pitch so the role compensated is going to be the role it's funny how I did Pitch roll and then I did roll pitch that that's not good let me do pitch roll pitch roll so now pitch with the complimentary filter we're going to apply that comp complimentary filter well we're always going to Anchor things we are always going to Anchor things to the pitch from the accelerometer the accelerometer data because we always know that in the end that's the right answer so we're going to say that we're going to always be listening to that but we don't trust it in the short term so we are going to put kind of like this would be sort of like a low pass filter on it we're only going to trust it to half of a percent which means we've got to keep seeing results from it before we trust it okay like that and then what are we going to get our shortterm data from our short-term data is going to be 99.5% n95 and that is going to be what that that is going to be here's where we apply the complimentary filter from the roll comp uh comp uh the the complimentary filter on the roll okay that's what we're calculating a roll what am I saying pitch that would give a jumbled up mess wouldn't it the comp the uh complimentary filter on the pitch what it was before plus what the X gyro which is going to give me the angular velocity and then times what times T Loop the time that it takes to go through the loop so it's the old complimentary filter Value Plus the angular velocity times the loop time and then that whole thing I'm going to trust to 99.5% but then for 05% I'm always going to look back at my friend the accelerometer and so I think this one is going to be very similar so I think I'll just cut it and paste it here like this and roll comp is going to be roll if this doesn't make sense go back and watch the last few lesson roll and then this would be the Y Cho okay now I've got this Loop time but if I'm going to use Loop time I better Define Loop time so I'm going to come up here here and before my Loop I'm going to just set it I've got to initialize it t Loop is equal to zero and while I'm doing that I better also initialize roll with the complimentary filter is zero and pit pitch with the complimentary filter is zero and now I've got to start my stopwatch so I've got to start my stopwatch so how do I do that T start I'm going to start timing is equal to time. ticks in milliseconds like that that's the number of millisec seconds since the epoch I've started my timer okay now I've got to end my timer all the way down here at the bottom and I don't think I'm going to put any delay in this one this time CU I've got updating that screen is going to take enough delay so I'm not going to add any other delays in there so now what I'm going to do is I'm going to say t stop and that similarly is going to be time do tick uncore milliseconds like that and that's the time since the epoch now the EPO was something like January 1st midnight 1970 something like that okay but you don't have to worry because I'm subtracting the two so really what I'm getting here is the lapse time and specifically what that Loop time would be that would be equal to T stop that I just measured minus t start that I measured at the first of the loop this reports in milliseconds I want to work in seconds so I need to multiply milliseconds by1 to have my units now in seconds hopefully that makes sense okay so look I think I've got this I've got this pitch compensated roll compensated okay let's just see if that is going to work I just want to kind of look at it here and just make sure that I'm getting kind of good uh good data and let's see do I do a print in here anywhere I think I'll just turn this print on and then what I'm just going to do is I'm going to have uh pitch from the uh accelerometer and then I'm going to have roll from the accelerometer and then this is going to be pitch from the accelerometer and this is going to be roll from the accelerometer and then I am going to get uh I'm going to then get pitch compensated and then I am going to get pitch compensated okay and then I'm going to get roll compensated and then I'm going to print roll compensated so what I should see now is does it work and is the filter working like I would like it to so let's go ahead and run this find out how many errors PC isn't defined oh yeah yeah so this is uh pitch accelerometer roll accelerometer and then this is pitch compensated and then this is roll compensated like that okay that's why I like to run my programs as I'm going pitch degrees isn't defined in line 4 45 okay and what I need to do here is I need to change this to my new variable name which is compensated and roll compensated sometime when you borrow old code you've got to adjust things a little bit okay now let's come in and run it let's see what other problems we have here roll comp needs an uppercase C all right here we go okay pitch degrees again in line 50 we use that thing again in line 50 we're just going to take that whole line out because we don't need other print statements slowing us up in here okay and same thing here pitch degrees should be pitch compensated because that's our final that's our final corrected value pitch compensated and then roll compensated using the compliment filter and now let's go okay we are off to the races and so let me see if I can make this big here and we seem to have something not real good happening with green and green is the uh green is the uh pitch from the accelerometer how could we have an error so quickly in here like that so it really looks like that that uh pitch gyro is drifting and it is just taking off to the races and so much so so much so so that this uh tying back to our anchor the accelerometer at at a half of a percent isn't enough because that runaway gyro is taking us off course however I think there might be something else going on let's just let's try running it again and let's see what happens uh see if it happens again okay yeah that green which is the compensated pitch that compensated pitch is taking off the pitch is working but the compensated pitch is taking off let's look at roll also I see I have another problem in roll and we had corrected this uh last week but we need to do it again here you see that the the roll that we're getting from the accelerometer is opposite from the roll that we're getting from the Gyros so remember what we did to fix that we came up here to the mpu gyro and we made that Nega so let's see if we can take care of that okay now that's looking good that's looking good but now we still have that really big drift problem this is what's happening what's happened this week from last week we've added this thing where we're going through this big circle 360 times to draw the circle every step and then we're also moving all of this data to the frame buffer and then we're sending it a command to show and all of that is really really really impacting our timing okay all of that is really really really impacting our timing so yeah every single time through the loop we want to calculate this thing but we don't want to do all this other stuff every time because you know for us to get a a good measurement we're measuring the angular velocity and multiplying it by the change in time and then adding that to our previous angular position and therefore we've got to be real Snappy and we've got to be attending to that calculation and we can't be off doing all this other stuff so what I'm going to try here I'm going to say I'm going to say count is equal to count + one so I'm going to put a little counter here and I'm not going to do the print and I'm not going to do the display every single time so I'm going to say now if CNT equal equal like let's just do it every 10th time and see how that works then if count is equal to 10 then we'll do all this other stuff but it'll only be every 10th time that will draw the circle and do the printing and do the showing like that okay and then down here we need to reset the counter to count equals z like that and now let's run this thing and see if that you know kind of kind of allows us to have a little bit more accurate timing and not be out fooling with the displaying and the printing so much so let's look at it count isn't defined yep up here I've got to initialize it CNT equals zero so now let's come in and run it okay the green is kind of taking off again but not nearly so bad okay okay it looks like it's kind of stabilizing at about 10° instead of 50° what if we uh what if we took this and said like every 15 then that would allow us to keep track of our timing even more precisely okay it went to about yeah I think this is going to be a little bit better doing it like this okay but now what you see is the problem here is now the display is getting kind of sluggish so we're going to have to go back to 10 and when we go back to 10 what are we going to have when we go back to 10 what are we going to have hopefully you guys see I'm kind of letting you s think through this on purpose okay so now if I look at that green that green is the the complimentary filter on the pitch axis and it looks like we've got about a 10° error what kind kind of error a longterm steady state error how do we deal with a long-term steady state error the little trick that we used last week that I had not added in this week yet right it was creating that idea of an integrated error steady state error and then squeezing that error out so let's go back and let's do that okay let's squeeze out that integrated error and what we're going to do is we're going to come up here and we're going to be good and do it this time before we eror we're going to have an error in the pitch and that's going to be start out at zero and an error in the rle and that's going to start out at zero like that okay now we are going to come down here and now what we're going to do is we're going to say what is that error p and what is that error uh rle okay what are those two errors let's work on that did I do it up here yeah I put it there okay so now let's do it the error P right we want to keep adding the error we're carrying the whole burden of all the errors and what do we do what does that mean we're going to take the error before and now we're going to add to it so every time there's a little error it's like our burden gets bigger because we're accumulating the sum of all the errors so the new error in the pitch is the old error plus what plus what the pitch from the accelerometer which is the exact answer minus the pitch with the complimentary filter and then how long did we have that error T Loop like that and now the error R is going to be equal to error roll plus what the exact answer roll accelerometer minus roll comp times T Loop now what we need to do is we need to go back and we need to add that error back in or some fraction of that error back in and then we're in effect we're squeezing out that error so let's say that that's going to be plus pitch uh plus error pitch and let's just say times point uh 0.002 so it's going to take a little while to squeeze it out but it will always be squeezing out okay now plus error roll times 0.002 now if we're we're doing like point every time through the loop we're taking out 2% of the error of that long-term error we're squeezing it out well if we squeezed it out faster it would get out faster but then it would overshoot and have to come back and it would sit there and oscillate and so let's try 0.02 and see how that looks let's see if we can squeeze it out reasonably now remember what we're going to see here is we're going to see that pitch I think was it green was the pitch we're going to see it start going away because of that drift term it's going to start running away because of that Thrift term in the gyro but then this error compensation is going to say whoa slow down not so fast let's come back to where we know we're supposed to be so let's watch it here okay so what we're going to do is we're going to turn it on and there it goes I'm going to let it auto scale and then I'll run it again so that we can see it a little better okay now we're watching The Green right we're watching The Green that's the pitch compensated and what does it do it's running away but now our error correction says Whoa you see it didn't get to -10 that error correction that longterm squeezing out of the error brings the uh brings the green it's going to bring it back to the blue okay there wasn't the same problem there wasn't the same problem in the roll it's just in the pitch and I find that interesting it makes me kind of Wonder is something going Goofy with my sensor but what did we do we compensated it out now let's see how this thing behaves okay so I'm going to pitch and look at that that is a beautiful pitch okay and the compensated caught up with the accelerometer beautifully okay beautifully working well now let's see how we're doing on vibration so you're going to see the blue vibrate but the green is Rock Solid the blue vibrates but the green is Rock Solid now let's try roll they're going in the right direction they're looking really good okay and now let's see what happens you see the Orange is from the accelerometer and it's all over the place but the red is holding steady okay you see the red is holding steady and the red is not misled by the vibration you see how the red is very steady okay okay that looks really good right that looks really good I mean that is saying that our device is behaving as it should behave I mean the raw data is behaving like it should and I think those parameters are looking pretty good I think the parameters are looking pretty good you see it's nice probably you guys if you could see it real time with me it would probably be a little better so you see I roll and that uh red is just really nice in the roll I'm getting very smooth data and the pitch is behaving very well too and then when it just sits there all those values are going back to zero okay that is looking just really really really good but now what we haven't done is we haven't gone in and really looked at the uh we haven't gone in and looked at the display to see what's what's happening on the display okay so what I'm going to do to look at that display at this point I don't want to slow this thing up any further than it already is and so I'm going to take the print statement out and then I don't have any delays in there that's good uh let's see I'm wondering if I could even uh say the you know that we go 0o to 360 what if we tried to go in steps of two and then maybe that would speed that part up a little B bit so we've done just a couple of little things I guess if I want to check the two thing I need to put the print back in we're just going to kind of tweak this a little bit here so we're going to come in and I'm going to run it okay yeah I think it might be running quicker now okay yeah I do think that that not spending so much time creating that Circle might have sped it up a little bit okay so now let's see now I'm going to go I'm going to take the print out like I said I'm going to take the print out as I said and now I'm going to stop okay and now this time when I run it we're going to come over here and let's look at this okay let's look at this and now let's run it okay and so now this would be kind of like our final device right now what do you see the bubbles going off to the right why that is the drift in that uh pitch gyro that's the drift in that pitch gyro and which gyro was that that was the uh that was the X gyro okay that was the the X gyro so it's drifting off but then our error compensation drives the error out and now we are here and this is saying that we have a roll of minus 1.2 and sometimes that's just you see that this isn't exactly this kind of wobbles a little bit so you could be off by a degree but now let's go nose up and when we go nose up boom look at that very very stable nose down nose up it says we're at 10° which looks about right we'll go back to 0 degrees you see and it's not drifting around like it was right when we started the program that is getting our control system locked in and you could see that it took 10 15 seconds to do that and now let's look at roll the roll looks really good let's look at roll and pitch and that looks really good and man look at this this thing is behaving extremely well and what you're going to say maybe is you're going to say well it looks like that it is behaving about like it was when you started okay but what did we do when we started we were vibrating and that bubble was moving all over the place now when we vibrate what happens that bubble is rock solid or if I tilt it you see Rock Solid I come back and it pops right back boom guys man I hope I really hope that I did not make you guys weary by going through this but you see I just know the tools that I had as an engineer that were just like magic I want you to get those tools in your toolbx and this project was not about the mpu 6050 it was about thinking like an engineer it was about thinking like an engineer and you're never going to get a perfect sensor that gives you perfect data so you've got to have these tools to work in the real world where the data is never perfect what did we learn we learned how to use a low pass filter we learned how to use a highpass filter we learned how to do a complimentary filter and then we learned how to squeeze out long-term steady state errors and those are going to be useful no matter what type of project you you pursue in the future anything that's going to have a sensor those are going to be useful okay so now let's talk a little bit that's all I'm going to do with this project because I think we've had a lot of fun we've learned a lot it's time to move on to something new but what are some directions that you could go with this one direction that I could think that you could go with it would be to have this tilt meter such that you were operating it as a server and you were interacting with it wirelessly so like that if you're doing something in one place you could be in another place and monitoring what the level or what the Tilt was that might be an interesting thing to take this and do Wi-Fi like I say I'm not going to go there because you guys are probably already we weary with me on that but that's One Direction you could go another Direction you could go you guys that have done Fusion 360 and have a 3D printer what you could do is ah there was one other thing I should show you here the final little step here I'll do real quick is we can save the pro we can save the program as Main and so let me come over here and what I'm going to do is I am going to save this on the Raspberry Pi Pico as main.py okay I'm going to save it as main.py okay so there it is and now what I want you to watch is I am going to come over here and now I'm going to unplug it and now I'm going to get my handy little battery and the really slick little power control unit that came in the sunfounder kit and I'm going to plug that in and now what we can see is completely untethered this thing has come to life you can see it's going to take 10 or 15 seconds for it to calibrate out I think that the other thing I would do to this program you guys can do it if you want you could not display the first 10 or 15 seconds and just say calibrating and that way you don't confuse them because once it gets here it works but then you see look at this wow we are mobile We Are Wireless and we are getting really perfectly Rock Solid operation so what did I say well I think I unplugged that as I was shaking it and it restarted this I found this connection can be a little loose and I found like putting a rubber band around it so that it doesn't uh it doesn't glitch out is a good thing to do so one thing you could do is make this Wireless and report back to a python running on your uh PC or on your phone that would be one thing the other thing that you could do is you could you guys that have done Fusion 360 and have a 3D printer you could maybe get a small breadboard and then create a little case and then you could have sort of like a portable unit that you could carry around in your pocket and maybe actually do some useful things with it okay man I like I say I worry that I've made you guys weary you leave me feedback below do you just want to just write simple programs or do you really want to look under the hood and understand what it's like to solve real world problems where you go from something working on your benchtop to something that would actually work in a real world uh application let me know if I went a bridge too far on this or if you really enjoy getting down into the nitty-gritty of this level of detail let me know guys I hope you're having as much fun taking these classes as I am making them uh I want to take a second and give a shout out to you guys who are helping me out over at patreon you guys are keeping me in the game it's uh YouTube has made it clear that they don't like Grumpy Old Men talking about educational type of things they want silly short little clips of people doing silly things and that's where their advertising revenue is going and that's where they're trying to steer Their audience and so I'm really doing very poorly as far as the advertising revenue and stuff you guys that are supporting on me on patreon you the guys that keep me coming back week after week after week thank you you all can also help me by giving me a thumbs up on the video also helps if you leave a comment down below if you haven't already subscribed to the channel when you do ring that Bell so you'll get notifications When Future lessons drop and as always share this video with other people because the world needs more people coding and fewer people sitting around watching silly cat videos Paul mcorder with top teo.men [Music]