Added simulator for IMU telemetry messages.
Some Debug output adaptions.