Interpolator and mutex
interpolator - lock problem
fixing a part
pre mensa
reaching target
interpolator - seems unnecessary
Working on calculateThrust
still on thrust-calculation...
Calculate thrust in comments
calculateThrust as defined
Interpolator, working on calculateThrust

getDistanceZ in Position6DOF added.
Merge branch 'master' into stabilize
Merge remote-tracking branch 'origin/master' into stabilize
minor fixes, interpolator
compiling - calculateThrust
Merge remote-tracking branch 'origin/stabilize'