Fixing integration

Removed recursive calls and some fix for firmware 
Please remove qDebug.