Minor cosmetic code formatting. Initial work to report frame angles and Mavlink errors

Hi.

I believe that everything is working as normal. Nothing changed on the code, and added changes to get frame angles and error codes are to be followed with additional code.

I'm thinking on combining existing telemetry values, to get more data down. Also I have to investigate the A3 and A4.