Minor bugfix and tuning for first tests on real unit