Added IMU data to 50Hz loop and addeding logging to controll callback.  Currently, the callback is never entered.  Disconnects after sending Ctrl.