code restructuring and support of MultiWii Serial Protocol