Fix roll, pitch PID output
Tune the pitch PID parameters