Implementing a new position and attitude controller

Got this post