This an autopilot algorithm for teensy with MPU6050, and send out PWM signal to four speed controllers. It was designed to work with the visual inertial sensor fusion framework in https://github.com/Changliu52/vi_ekf.
Related publication Liu, Chang, Stephen D. Prior, and James P. Scanlan. "Design and Implementation of a Low Cost Mini Quadrotor for Vision Based Maneuvers in GPS Denied Environments." Unmanned Systems 4.03 (2016): 185-196. http://www.worldscientific.com/doi/abs/10.1142/S2301385016500059
This project was created within an academic research environment, thus should be considered as EXPERIMENTAL code. There may be bugs and deficiencies in the code, so please adjust expectations accordingly.