Por partes....
El codigo que puse adjunto unicamente maneja la señal procedente del LSM303 y obtiene el error (+/-) que hay entre un rumbo guardado mediante un boton y los cambios que puede haber posteriores a la pulsacion.
Quiero decir que en ese codigo NO hay nada relativo a salidas PWM o SERVOS u otros usos.
Ese codigo SOLO es aplicable al LSM303... No se que sensor usas pero en el codigo que puse no sera muy dificil adaptarlo para el tuyo.
Una vez obtengas de tu sensor un error a babor o estribor de un rumbo marcado con un boton conectado a TU placa, lo siguiente sería el traducir ese error a valores de PWM con los que hacer girar tu servo en un sentido u otro según la tolerancia del error que quieras manejar.
En un dibujo mas abajo explico lo del PWM.
Ya dije que es algo parecido a un taladro apilas con control de velocidad.
Para explicarlo con el taladro que todos tenemos en casa:
Si el error es positivo atornillamos a derechas (el taladro lleva una palanquita para ello) y la velocidad de giro la controlas con el gatillo (eso es el PWM) con velocidad de lento a maximas revoluciones.
Si el error es negativo atornillamos a izquierdas (la palanquita al contrario) y de igual modo vamos de menos al tope de revoluciones.
Yendo de nuevo a tu servo:
Aqui hay una rutina que controla el servo
http://arduino.cc/en/Tutorial/Sweep
En el codigo de esa pagina hay una variable llamada "pos" que tiene un valor inicial de 0 (cero),
Si ese codigo lo incorporas al de deteccion de error y sustituyes el valor de cero por el del rumbo seleccionado podría el servo moverse a un lado u otro con el error que hubiese despues de fijar el rumbo.
En fin... que hay materia para entretenerse. Y no es tan complicado...
Yo usé codigo copiado de varios sitios de Internet y luego pregunté y pregunté hasta llegar a tener lo que queria...
Saludos y recuperate pronto.