Hi, I have been developing this code since May as well as a custom Arduino-Like Board, It is a 4 motor simple drone that interacts with the MPU 6050. Please review what I have written and please give me feedback on either suggestions or modifications to the code. Thanks a lot, I appreciate it!
You need to use a proper PID loop for the stabilization which I don't see in here. I don't think what you have will stabilize the drone (at least not well) until you add a PID loop for each axis. So you need a PID loop for pitch, roll and yaw (usually don't need it for throttle/altitude).