Bugfixes for servo control and some other minor firmware issues
version bump: 1.6 beta