Code to control QCopter

I build my own QCopter but i have a huge problem to control him :). I tried a lot of stabilisation code from various projects, but none works good enough. My friend recommended me ArduPilot, because it’s software works good and smooth in a lot of projects. But repository is HUGE and code have mixed functionality so surgical excition only this part responsible only for calculate engines power to fly correctly is too hard for me.

I need function or library where i provide to this pitch and roll angles from gyroscope, yaw, pitch, roll
and throttle where i want to fly and on output i gets 4 values (e.g. 0-1000) that represents engines power i must set to move (or hover in the air if pitch and roll are 0). Could anyone send me this? Project is blocked about month because of this problem

Sorry for my bad english

ArduCopter code already does all that for you. you can use lua-scripting, mavros or dronekit-python to program it. But I do not think you need to to. Just download the firmware as test it. It works great.

As I told i don’t need all the functionality ardupilot dispatch. I need only this part of code responsible for stabilisation and fly. Read carefully

PS. I don’t have even arduino. I’m writing this for completely another platform, but in c++. So i can use code parts easily but program as a whole won’t work. But it doesn’t matter