multirotor_att_control: cleanup, some refactoring