Calculate thrust in comments
calculateThrust as defined
Interpolator, working on calculateThrust

getDistanceZ in Position6DOF added.