Problem with too low required thrust solved. Working for both velocity
and attitude mode