Modified the arduino code to support basic mavlink stuff.

Still needs to be tested
Added Better Static Mapping Algorithm using Moore-Penrose Pseudo
inverses.