Wenn sonst alles geht, also zumindest stable und althold, letzteres auch bei zügigen Fliegen....kein Zucken, kein Pulsieren.
Mich würde trotzdem noch ein Vibrationslog interessieren.
Generell ein Log wäre gut. Wieviel Sats und was für ein HDOP im freien?
GPS-errors?
Ich denke, das ist kein grundlegendes PID Problem.
Eventuell mal erase mit arduino und neu flashen und von vorn beginnen.