Implemented helper function for autofocus algorithm
Fixed buffer requeueing routine