Measuring Angles with MPU6050 Accelerometer for Quadcopter Stabilization

[00:00:00]welcome to the next video in our series explaining how to build your own Arduino quadcopter drone in this video we will explore how you can measure angles with the help of the mpu 6050 gyroscope and accelerometer which will subsequently be used to design a different type of flight controller in the previous videos we designed a flight controller to stabilize the quadcopter based on the rotation rates in degrees per second this means that you give a rotation rate commands to your radio transmitter for example if you move the stick and give it a pitch rate command of 10 degrees per second the quadcopter will pitch 10 degrees each seconds if you release a stick after 6 seconds the pitch rotation rate commands will fall back to zero degrees per second causing the pitch angle of the quadcopter to stop at 60 degrees however the quadcopter does not automatically go back to a level pitch angle of zero degrees with this type of controller for Pilots this type of controller is rather difficult because it does not stabilize the quadcopter to a level position when releasing the sticks to go to such a controller it is necessary to give an angle commands for example when you give a 45 degree pitch command with the Stick of the radio transmitter the controller will cause the pitch angle of the quadcopter to go as quick as possible to 45 degrees when you release the stick the pitch angle command goes back to 0 degrees and the quadcopter's pitch angle goes back to zero degrees as well the controller which gives you direct control over the roll and Pitch angles of your quadcopter is often called a stabilized mode controller as opposed to a rate or Acro mode controller to build a stabilized mode controller we first need to be able to measure angles correctly a very simple approach to measure angles can be found by simply integrating the rotation rates to get the actual angle remember that K holds the number of the current iteration and TS is the iteration length which is equal to 4 milliseconds in a 250 Hertz Loop if you were discretizing integral for using the Arduino codes you would need to add the rotation rate multiplied by the iteration length to the angle of the previous iteration in order to get the angle of the current iteration if that sounds too simple to be true that's unfortunately because it is this approach has two major problems you keep dragging all measurement errors from previous iterations with you causing the calculated angle to drift very fast another issue is a change of angles during your movement suppress the quadcopter is flying with a constant pitch angle of 45 degrees when you are around the z-axis without any pitch rotation rate around the y-axis the pitch angle will nonetheless decrease or increase because the y-axis changes its direction as well this is equally true for the rule rotation rate that is why you need to use a totally different approach for measuring angles and for which you will use the accelerometer built in your mpu 6050 as the name implies the accelerometer measures the acceleration of the sensor along the X Y and Z directions from basic physics you remember that we experience a gravitational acceleration and anywhere on Earth and that this gravitational acceleration is equal to the gravitational constant 1G or 9.81 meters per square seconds this means that when you let your mpu 6050 sensor lie flat on a table without moving it the measurement of the acceleration along the zip direction or x z is equal to 1G the acceleration along the X and Y axis will be zero in this case similarly when you position the sensor such that one of the other axis lies perpendicular to the surface of the table the corresponding acceleration is also equal to 1g of course any other direction not along one of the three main axes will result in a non-zero acceleration value for all three directions through some clever mathematical equations this accelerometer property will enable you to calculate the exact Rule and Pitch angles of your quadcopter let's assume you roll around the x-axis until you reach the angle Theta rule to visualize this transformation a box bounded by the X Y and Z directions is sketched on the screen from your basic trigonometry knowledge you know that the tangent of the angle of a triangle is equal to the length of the opposite side divided by the length of the adhesion side in case of the angle Theta rule the length of the opposite side is equal to the acceleration in the y direction the length of the adjacent side can be calculated by the Pythagoras who finally giving you the equation to calculate the roll angle from the accelerometer values a similar strategy and reasoning can be applied for the pitch angles giving you a second equation to calculate the pitch angle from the accelerometer values and that's it you are now able to calculate the rule and Pitch angles from the accelerometer values now you need to transform this to a working code for this part you only need your TNC and mpu6050 and you can choose to test the code on a breadboard or directly on your assembled quadcopter we will take the code from the gyroscope derived in Parts 4 and 5 and add the necessary lines to read the accelerometer values first Define the accelerometer and roll and Pitch variables next start a function that will extract the signals from the mpu6050 and switch on the low pass filter seen in Project 4. now you need to configure the accelerometer outputs according to the mpu6050 register map the accelerometer configuration settings are stored in register 1C for this project you will choose a full-scale range of 8 G which corresponds to a value for the AFS scl setting of two or a 0 for B3 and a 1 4 bit 4. the other bits can be set to zero which gives a binary representation of 16 or a hexadecimal value of 10. the values of the accelerometer are located in the registers with hexadecimal numbers 3B 240. start writing to address 0 times 3B to indicate the first register and request 6 bytes from the address of the sensor 0 times 68 the accelerometer measurements are spread out over 3 times 2 registers with each 8 Bits repeat the same code for all accelerometer directions next configure the gyroscope output as seen in part 4 and extract the rotation rates to convert the accelerometer measurements from LSB to G remember that you configured the AFS scl setting to an LSB sensitivity of 4096 LSB per G to get the measurements in G just divide the measurement in LSB by 4096 LSB per g at the start of this part you learned how to calculate the roll and Pitch angles from the accelerometer values you can use these equations at this point but take into accounts that are acting hence calculated by Arduino returns a result in radians not in degrees to convert the angles from radians to degrees just divide the results by P divided by 180. start a communication with the gyroscope as seen in part 4 and print the accelerometer values now upload The Code by connecting the TNC to your computer and pressing the upload button in the Arduino IDE open the serial Monitor and watch the acceleration measurements in the X Y and Z directions on the screen you will see that the acceleration in the set direction is almost equal to 1G because the mpu6050 is lying flat on the table now tilt the sensor vertically in the y direction notice that the acceleration in the y direction becomes almost equal to 1 while the Z Direction goes back to zero repeat this for the X Direction calibration is once again necessary to correct these values as they are not exactly equal to one in the different directions this calibration needs to be done manually and will be different for each sensor just abstract or add the difference between the actual values of the accelerometer and one in lines 36 to 38 of the codes and we check that the acceleration values in all three directions are equal to one G when tilting the mpu 6050 now that you have calibrated the accelerometer replace the lines in the loop parts of the codes to print the roll and pitch angle and upload the code again instead of looking at the serial monitor we will use the serial plotter to visualize the roll and Pitch angles first you will roll the accelerometer in the positive direction up to an angle of 30 degrees the accelerometer values are very responsive and very accurate repeat this movement and subsequently perform a two positive and one negative pitch movement as well the results are excellent and very accurate when the sensor is level again the roll and Pitch angles are nearing zero however a problem appears when the accelerometer experiences vibrations which are inevitable when flying a quadcopter due to the presence and influence of the moving Motors let's simulate these Vibrations by hand you will notice a very sharp Peaks and drops in the measured pitch and roll angles because the amplitude of these vibrations will increase a lot during flight it is not feasible to build a good controller based on the accelerometer measurements because they are heavily disturbed by these vibrations integrating the rotation rates to obtain the angles is also possible and straightforward as we saw in the first part of this video this rotation rate measurements are not sensible to vibrations however you get an ever increasing error because you will add all measurement error first when integrating to obtain the angle the graph on the screen shows the measurements of the angle through rotation rate integration and accelerometer trigonometry for a stationary sensor and illustrates the ever increasing error with rotation rate integration and the sensitivity to vibrations with accelerometer measurements it is clear that the two methods of measuring and calculating angles are not suitable for a flight controller and that we will need a different solution in the next video I will show you how you can combine both measurements and get rid of their individual disadvantages by using a Kalman filter thank you for watching this video don't forget to subscribe if you like the series and remember that you can find all tutorials on YouTube and the full code on GitHub the manual which contains all expectations is available as well on GitHub if you need some more information thanks for watching and see you next time