lots of bug fixes

All controller services seem to work with dummy driver.
Only addWaypoints and sweep services remain untested.