Changed back to v2 algorithm

10% reduction in the PID values compared to the old quad settings