Estabilizador: Arduino nano + MPU6050

Primero que todo soy nuevo por aquí aunque llevo unos buenos días leyendo como un loco. Me presento un poco: tengo 22 años, estoy en Barcelona (aunque soy de Castellón) estudiando un grado aeroespacial esp. aeronavegacíon, estoy ya en tercero y el trabajo fin de grado se acerca.
Hace ya un tiempo que compre un arduino nano y varios sensores con la idea de algún día hacer algo... nunca he tenido tiempo.
Ahora que ya estoy de vacaciones me pareció interesante hacer algún proyecto, y surgió la idea de hacer un estabilizador, en principio muy sencillo con intención de complicarlo poco a poco, siendo realistas, en principio me conformo con equilibrar el ángulo de balanceo y cabeceo. Si esto sale bien pasara a implementar control de altura y rumbo (barómetro y magnetómetro).
Ya se que existen miles de controladores como Multiwii, Ardupilot.... pero como bien he dicho el proyecto fin de grado se acerca y mi intención por ahora es aprender desde cero Arduino para tener mas herramientas con las que trabajar en el fin de grado.

Vale, después de torrarla tanto con la presentación vamos al lio:

He estado como un loco empezando con esto, leer los datos del MPU6050 y pasarlos a ángulos que podamos entender y tengo algo decente ya. mis dudas:
-[1] He usado una libreria MPU6050 de un proyecto que encontré, mas q nada por la complicación del I2C. Alguien que domine del tema me puede guiar para hacer mi propia librería con lo básico, trabajo con un nano y la memoria es justa...
-[2] Tema de offset, leido por otros proyectos. Es muy simple encuentro un valor promedio en reposo del giroscopio q después lo resto al hacer el calculo, bien no?, el tema de offset para el acelerometro, no he visto nada por ahí, pero digo yo q se podría hacer también, o no es necesario?
-[3] Tema de signos de los angulos encontrados, no se por que los ángulos en x dan signos contrarios para el acelerometro y giroscopio, en cambio para los y están perfe.
-[4] Otra cosa que me tiene loco, como configuro mi MPU6050, cambiar sensibilidad y todo lo que se pueda hacer, no encuentro ninguna función en la librería para ello....

Pues estas son las dudas de lo que tengo hecho hasta el momento, ahora estoy introduciéndome en el PID, hice un intento pero no me queda del todo claro. Voy a seguir investigando sobre PID a la espera de vuestra ayuda en el tema del MPU6050.

Muchas gracias por el interés, cualquier ayuda se agradece, tened en cuenta que hace 4 días no sabia NADA de arduino ( pero bastante de programación Matlab y C#).
Si alguna buena persona cree que podría serme de gran ayuda y se compromete a ayudarme estos meses en medida de lo posible en este proyecto, prometo referenciar el el trabajo su esfuerzo y compartir méritos. por supuesto todos los derechos sobro el código. XD XD

Adjunto el código hasta el momento.

NanoPilot.zip (14.3 KB)