cleaned up the init (mma) a bit
moved MPU initialization to own function
cleaned up initialization code