Use pointer casting instead of memcpy to data container.
Save some calculation if DEBUG not activated
Adapted to send one IMU message

since we receive them all together now