Finished implementing the initial madgwick seed orientation logic