Restarting the project

Adjusted for use with FreeIMU, re-written most of Quad.cpp