adapted attitude controller to use new attitude estimator and some changes after testing. Flying in attitude mode is tested and working