QuadCopter code running on Xmega128A1