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)