hm, das ist echt blöd. Ich würde als nächstes im code nacheinander verschiedene parameter abschalten. Also z.B. erstmal verhindern, dass die RC-Signale für yaw ans servo weitergeleitet werden. Gyro etc bleibt weiter aktiv. Als nächstes dann RC-Signale aktiv lassen und alle Gyrosignale für yaw abschalten.
Also einmal hier lf_yaw auf null setzen um RC auszuschalten:
Yaw_diff = Sempf(yawchannel) * Lf_yaw

Und danach einmal hier das Gyro abschalten:
Yaw_gyro=0
Yaw_diff = Yaw_diff - Yaw_gyro
Yaw_gyro_i = Yaw_gyro_i + Yaw_diff

Das geht wohl auch übers GUI...
Gut wäre halt mal ein Oszilloskop....
Außerdem ist das verlegen des Servokabels nahe der Motorkabel schlecht, und ein Ferritkern nahe am Servo hat bei mir wenigstens psychologisch geholfen...