i2c update and HIL testing

Added mode for altitude hold for AIAA testing
Removed wait instructions in i2c addressing to speed up transactions.
Also allows for disconnected sensors to be properly handled.
Added in control data to SD write routine.
MAVLink output will no longer function if port declarations are out of
range.