Added code to Tricopter.c