Changed a lot in code structure. IMU split into two tasks.