Small cleaning in main.c and imu.c