Update focus() to work on GPS coordinates

[aaron4522]

In camera.py, focus() works by moving the camera until a detection is in the center of the frame. Because the boat is constantly moving, there is a decent chance that detection will not be in frame, especially if there is a significant time difference between capture() and focus() being called.

• Support focusing based on GPS
  • Take into account the boat's heading and distance to the desired GPS marker and move the servos to focus on that point
  • Boat may keel over which can mess with servo yaw/pitch not being aligned to the horizon. Correcting for this would be great but difficult.
  • If its impossible for the camera servos to focus on the GPS point alone (ie. the point is behind the boat and it would have to turn the rudder to see it) then raise an exception

Achieving this will give us more precision for search/precision nav.