Added possibility to log pid control values
Integrate local position only when quality is good, pos controller somewhat working