Fixed a bit of derp from the last merge. UART telemetry TX is commented out until we figure out why it is hanging.