adapted attitude controller from ros code