Autofocus robustness

Trying to figure out how to make autofocus more robust, added a rough +
fine autofocus routine