hi
even if you upload another FC firmware won't work since , firmware is calibrated on dynamic model... and need to be concurrent with the physical object you are dealing with.
other suggestions:
1) i strongly encourage to go back to Madgwick since he works with quaternions and avoid some...
It seems more a programming problem, than a structural problem, your quad is not correcting errors how it should... seems very slow doing that, and he seems missing D component in PID.
what frequency are you running your PID ? how did you set up your K gain values for your PID?
what algorithm...