Interpolator - rotation now with sign
Interpolator - calculation of roll and pitch
Controller - some missing brackets...
Interpolator - half rp-values if closeToTarget