Restructure project and use 9-axis sensor fusion