New algorithm for earth controller, more flexible