Removing the trim from the motors. I think I can configure the ESCs so that trim is not needed.
Accidentally submitted Serial code.

My last commit also contained code to invert my IMU again. It now assumed that the header pins are at the front of the copter and the SMDs are facing the sky.