roll : rocket heading direction
pitch : y axis of IMU
yaw : x axis of IMU

servo pin(PWM available) 3, 5, 6, 9
3,5 => yaw control
6,9 => pitch control

there is some variables like
**********************************
#define r_ac 1
#define py_ac 1.5
#define pitch_yaw_range 3
#define roll_range 0

int first=1, start=0;
int count=0, pitch=0, roll=0, yaw=0;
float z_roll0=0, y_pitch0=0, x_yaw0=0;
**********************************
'ac' means angle constant. It is coefficient for servo control
'range' is a boundary for control enable/disable

This code use DMP of MPU6050, and it takes some time for calibration.
Typically, it takes about 15 seconds. However, due to safety reason, you should take it for about 30 seconds.
After start, the programs wait 3 seconds to be stable.
During calibration, please don't move it.
After calibration, it turn on a led. (pin 4, you can change moderately)