Working version with appropiate gains in PID.
Cleaned unused code in quad control.
Using gyro measurements as derivative.