Schau dir mal die Sourcen von dem Simonk(irby) oder Blheli an.
Sind zwar eher für Quads (wegen der Ansteuerfrequenz) bzw Heli/Quad (Blheli) gedacht aber das Funktionsprinzip ist ja das gleiche.