Clean up L3GD20, add error flag set by I2C comm errors
Separate motor control into functions based on failsafe state and gyro enable state