Refactor working for fixed attitude cartesian