Modify the code that calculates the angles from the IMU to solve some bugs and to simplify the code