robvoi,

für diese Art von Regelungen nimmt man normalerweise eine Kaskaden-Regelung. Aber um Dir raten zu können, müsste ich erst wissen:
1. benutzt Du Deine bisherigen beiden Regler als getrennte Drehzahlregler (je einer pro Kette)?
2. welche Messwerte liefert der Kompass (den Winkel oder auch den cos und den sin des Winkels)?
3. kannst Du Vektorrechnung (Skalarprodukt, Vektorprodukt)?

Ciao,
mare_crisium