I am testing a setup very similar but I’m having problems with vibration. I think my problem is my engines and propellers.
When I select altitude hold the quad accelerates to full power. My GPS support was broken for this reason when my quad shot in the air and hit a power cable.
motors: 800kV 2217 rctimer
props: 10x47 carbon fiber
5000 mah battery 4S