Correction of the issue #24
 / This modifications works on RoboController, but they must be tested on uNav by @rbonghi