Code Cleanliness And Organization

My main concern is performance. I want my quadrotor to perform optimally, so iI guess I need to rephrase.

What, if anything do I need to do to make sure my code isn't unnecessarily large?

The serial print is just for the prototyping to make sure everything is working properly, in the end i will end up using this data for the autopilot and other autonomous features. The reason why i am concerned about the code, intensity, for the lack of a better term, is that i plan on using several more sensors and i don't want the ATMega to be strained and cause sensor lag. Am i worrying for nothing?