Controling a servo with mpu6050

Hi guys,
I am working on a project in which i need to control and stabilize a rocket, launched vertically that will plane. In order to grant a proper stabilization, i was trying to figure out if is there any way to provide the servo with information from the 3 axis of the mpu6050. The code i have now works perfectly only in 1 axis(either ROLL, PITCH or YAW), but not with 2 or 3 simultaneously.
Any help is welcome. Thanks!

mpu6050 code.txt (6.54 KB)

I began with code to hold a X/Y platform level when the base is moved and a MPU6050 and moved onto a MPU9250. The addition of magnetometer allowed the use of the MahonyQuaternionUpdate method, I then turn the Quaternions into Euler angles, finally turn the Euler angles into servo torque values to keep a platform level. To keep the magnetometer calibrated, I have a 2nd MPU9250 mounted 180 degrees, as referenced by the magnetometer, to the first MPU9250, as the platform moves around. It would not be difficult to hold the Z axis pointed N as the platform moves around.

I suggest you look over this fellows work:

If you use the MPU9250, I found that the MahonyQuaternionUpdate method works a lot better if values are entered for Kp and Ki. These are the values, I found that work quite well for Kp and Ki for the MPU9250. // #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral

define Kp 7.50f

define Ki 1.7f

Reads like a fun project. Good luck.

I think that you will find many forum members are unwilling to help with making guided missiles. Many governments frown on people making guided missiles.

@unilord - in many jurisdictions, a dynamically stabilised rocket is a guided rocket.

No ifs. No buts.