code refactoring and small bug fix
Merge branch 'udoo' of https://github.com/Wyliodrin/libwyliodrin into udoo