Improved sensor reading architecture
Added initial code for IMU reading (not working properly)