Testcode for quadcopter stabilizer
Modiefied for dominik - removed stabilizer