Proyecto: brújula electrónica

En este proyecto vamos a ver una simple aplicación del uso del conversor AD del  PIC. Se trata de escribir el código base de una brújula electrónica. La idea es disponer de dos sensores magnéticos alineados en dos ejes (A y B) perpendiculares en el plano horizontal, cada uno de ellos midiendo la correspondiente componente del campo magnético terrestre.

Con el conversor AD tomaremos dos medidas (ma y mb) "simultáneas" (separadas por unos pocos usec) de ambos sensores. A partir de ellas hallaremos el ángulo relativo (theta) entre la orientación del eje A de nuestra brújula y el norte magnético.  Además de usar el conversor AD, el proyecto nos servirá para introducir el tema de optimización de rutinas matemáticas cuando estamos usando microcontroladores. En particular en este proyecto tenemos que usar la función arco tangente para obtener el ángulo a partir de las medidas ma y mb. Veremos una implementación aproximada de dicha función, adecuada para la precisión requerida en este proyecto y con bastante menos carga (tanto de código como computacional) que la rutina matemática (atan2) suministrada con el compilador.

El siguiente cutrevideo trata de dar una idea del resultado final.  

Código asociado a esta entrada:  brujula1.c, brujula2.c

------------------------------------------------------------------------------------

Hardware:

Para el sensor, hemos usado una placa de SparkFun que usa el sensor HMC1052L de Honeywell. El sensor mide el campo mágnetico en dos ejes A y B perpendiculares. Un circuito amplificador multiplica por 100 dichas señales y las pone a disposición del usuario en dos pines etiquetados como OUT1 y OUT2. Dichas salidas se conectan a los pines RA0 y RA1 del micro, usados como entradas analógicas. Los otros contactos son alimentación a 5V (Vcc) y masa (GND). Un quinto pin etiquetado como SET sirve para "recondicionar" el sensor si se ha visto sometido a un campo excesivo. En este montaje no lo he usado y está conectado a masa.

SparkFun ya no vende esta placa pero podéis encontrar otras similares, aunque algunas de ellas ya no dan una salida analógica, sino que comunican sus datos a través de una comunicación serie (SPI o similares).

El único otro hardware usado será el puerto serie que usaremos para mandar los resultados al PC.

Calibración del sensor:

Antes de escribir el programa principal debemos considerar la cuestión de la calibración del sensor. La documentación nos indica que en ausencia de campo magnético en un eje el voltaje en la salida es nominalmente Vcc/2, lo que correspondería a un valor numérico de alrededor de 512 del ADC. A partir de ahí el voltaje puede subir (>512) o bajar (<512) si se detecta un campo magnético  positivo o negativo.

El sensor da un valor negativo (menor que Vcc/2 ) si le enfrentamos a un polo sur magnético:

--à A         S       ->  voltaje "negativo"  < 512

--à A         N       ->  voltaje "positivo"   > 512

Esto implica que el sensor dará un valor negativo cuando el eje A apunte al Norte magnético (que para aumentar la confusión es el equivalente a un polo Sur de un imán (y la razón por la que los polos norte de nuestras brújulas apuntan hacia él).  Para seguir la convención de la figura anterior deberemos cambiar el signo a las componentes medidas por el sensor.

El problema es que el valor "medio" de 512 es puramente nominal. En esta placa es obtenido con un divisor de voltaje por lo que puede variar ligeramente debido a la tolerancia de las resistencias.

Hay varias formas de fijar de forma más precisa este sesgo. Una de ellas es girar la placa hasta encontrar el voltaje mínimo en un eje, anotarlo, y girar la placa 180º hasta encontrar su máximo. La media de ambos valores será el "cero" buscado.

Para el canal A he encontrado un mínimo de 508 y un máximo de 534, lo que da un punto medio de  521.

Otro procedimiento es girar la placa durante un rato mientras calculamos hacemos una media de los valores medidos. De esta forma obtenemos unos valores medios de 521 y 522 para ambos canales respectivamente.

Como se observa el rango de valores en el que nos movemos es reducido (+/- 15 máximo). La consecuencia de esta baja resolución es que (en el peor de los casos) un cambio de 1 unidad en la medida de un eje puede suponer un cambio de hasta 1/15 rads (4º) en la orientación. De hecho la documentación del sensor recomienda usar un ADC de 12 a 16 bits si se requiere precisión. Indican que con un conversor de 8-10 bits nos encontramos limitados a una precisión de 3-5º.  Esto es compatible con los valores encontrados.

 Sotfware:

Una vez efectuada la calibración, el programa para nuestra brújula (por lo menos en su primera versión) será poco más de lo que vimos cuando presentamos el conversor AD.  Tras los  #include y #pragmas habituales hacemos algunas definiciones y usamos la función ADC_Read que escribimos para leer un canal.

#define send_CR putc((char)0x0D,stdout)

#define send_LF putc((char)0x0A,stdout)

#define CH_A  0   // Channel A in RA0

#define CH_B  1   // Channel B in RA1

#define USE_360   // (0º -> 360º) instead of (-180º -> 180º)

#define select_ADC(ch) { ADCON0 &= 0b11000011;  ADCON0 += (ch<<2); }

uint16 ADC_read(uint8 ch)

{

 uint16 res;

 select_ADC(ch);

 ADCON0bits.GO=1; while(ADCON0bits.GO);

 res = ADRESH; res=(res<<8)+ ADRESL;

 return res;

}

En el programa principal, configuraremos el puerto serie (para enviar resultados) y el conversor ADC:

void main(void)

  {

   int16 rr;

   int16 res1,res2;

   float rumbo;

   

   TRISC=0; PORTC=0;     

   // Configure ADC

   TRISAbits.TRISA0=1; TRISAbits.TRISA1=1;  // RA0,RA1 inputs

   OpenADC(ADC_FOSC_16 & ADC_RIGHT_JUST & ADC_4_TAD,   

           ADC_INT_OFF & ADC_VREFPLUS_VDD & ADC_VREFMINUS_VSS, 7);

   // Configure USART @ 9600 (20 MHz)

   OpenUSART(USART_TX_INT_OFF & USART_RX_INT_OFF & USART_ASYNCH_MODE &

             USART_EIGHT_BIT & USART_BRGH_HIGH , 129);

   puts("Electronic Compass (ver 0.0)"); send_CR;  // Send start message to USART

  

   while(1)

     {

      Delay10KTCYx(250); // 1/2 sec

      res1 = ADC_read(CH_A)-521; res2 = ADC_read(CH_B)-522;  

     

      res1=-res1; //res2=-res2;  // positive for eastern deviations

    PORTCbits.RC0=1;    

      rumbo=atan2((float)res2,(float)res1); rumbo=rumbo*57.2958; //degrees

      #ifdef USE_360

          if(rumbo<0) rumbo+=360; // 0-360

      #endif

      rr=(int16)rumbo;

      PORTCbits.RC0=0;

      printf("(CH_A=%4d) (CH_B=%4d) -> %d\n",res1,res2,rr); send_CR;          

     }

}

Tras la configuración entramos en un bucle while tomando 2 medidas por segundo del campo magnético en ambos ejes.  Tras restar los sesgos (521,522) a las medidas cambiamos su signo. Notad que el signo del canal B (res2) no cambia (o lo que es lo mismo cambia dos veces). El segundo cambio de signo es para seguir el criterio convencional de las brújulas de que si nuestro eje A apunta al este del norte magnético obtenemos una declinación positiva.

Una vez hechas estas correcciones basta llamar a la función arco tangente (atan2) para estimar el ángulo de discrepancia con el norte (o sur) magnético. Al contrario que la función atan(y/x) la función atan2(x,y) recibe los argumentos (x,y) por separado, lo que le permite discriminar en que cuadrante se encuentra el ángulo. La función atan2 devuelve su valor en radianes, por lo que multiplicamos por 57.2958 (= 180/pi) para obtener grados. Si queremos valores entre 0 y 360º deberemos corregir (sumar 360) si el ángulo resultante es negativo.

La captura de la derecha muestra la salida del puerto serie. El campo en el canal A es casi nulo, lo que indica que dicho eje es perpendicular al campo magnético, por lo que la declinación es de unos 90º. También se observa que dada la escasa resolución, un salto de 1 en el canal A supone unos 4º de cambio, pasando de 86º a 90º sin saltos intermedios.

Es posible reducir estos saltos si en lugar de tomar una medida tomamos una serie (8 o 10) medidas muy cercanas entre si, sumando sus resultados y llamando a la función atan2 con los resultados acumulados. No haría falta calcular la media, ya que la función atan2 no cambia si ambos argumentos están multiplicados por el mismo valor.  

La segunda captura muestra la salida usando la acumulación de 8 valores. El valor del canal B es ahora de 122-124 en vez de 15-16 (15 x 8 = 120).  Ahora la salida muestra menos saltos, mostrando cambios de alrededor de 1º. La contrapartida es que cada medida no corresponde a un instante de tiempo, sino a un promedio durante el tiempo que hemos tardado en tomar las 8 medidas.

   

Optimización:

El código anterior es sencillo porque su principal complicación, el cálculo del arco tangente, ha sido llevada a cabo por la función atan2 del compilador que usa aritmética de coma flotante. 

El problema (no en esta, pero si en otras aplicaciones) es que usar este tipo de funciones tiene un alto coste en un microcontrolador de 8 bits. Este coste es debido a la alta precisión suministrada por la función atan2. 

La salida de dicha función tiene una precisión del orden de 10^-8 (la precisión de la representación en coma flotante usada, usando 4 bytes por número real). Este error es del orden de una millonésima de grado. Dicha precisión es totalmente innecesaria, dada la imprecisión de los datos de partida de nuestros sensores (como ya vimos, nuestra resolución no es mucho mejor de un grado).

Esta innecesaria precisión tiene un coste tanto en tamaño de código como en velocidad de ejecución. La función añade 2K words al tamaño del código, lo que podría ser un problema en muchos microcontroladores, o en un proyecto más complicado donde estemos cerca del límite de la memoria de programa del microcontrolador.

En cuanto al tiempo de ejecución, el pantallazo adjunto muestra la duración del computo del ángulo, mediante el socorrido método de poner a 1 la línea RC0 y bajarla tras terminar el cálculo. Vemos que tarda 1.75 msec. De hecho esta duración es variable, ya que dependiendo del ángulo resultante el cálculo puede tardar más o menos. Esto implica que sería imposible actualizar el ángulo más de unas 600 veces por segundo. En este proyecto esto no resulta un problema (ya que en una brújula electrónica basaría con refrescar la pantalla unas 10/20 veces por segundo) pero en otros puede ser insuficiente.

Si no deseamos tanta precisión y tenemos escasez de recursos podemos hacer nuestra propia implementación de la función, adaptándola a la precisión requerida. Una posibilidad es construir una tabla de valores, interpolando entre ellos. El tamaño de la tabla vendrá dictado por nuestros requerimientos de precisión (en este caso del orden de un grado).

Otra alternativa (la que veremos aquí) es construir una aproximación a la función atan2 usando aritmética entera (para que sea más rápida) usando un algoritmo de tipo CORDIC (COordinate Rotation DIgital Computer). También en este caso es posible ajustarse a la precisión requerida, no haciendo más trabajo del necesario.

Los algoritmos CORDIC  (http://en.wikipedia.org/wiki/CORDIC) se usan para calcular funciones trigonométricas (y otras) basándose en rotaciones de un vector en el plano complejo. En este caso, se parte de un vector de coordenadas (x,y) que forma un ángulo theta con la horizontal. Obtener dicho ángulo es nuestro objetivo. El algoritmo se divide en dos partes:

·          Se rota el vector hasta reducirlo a un ángulo más cercano a cero. La gracia es usar rotaciones con ángulos cuyas tangentes sean 1, 0.5, 0.25, etc.  (45.0º, 26.55º, 14.03º,  etc.). El interés de estos ángulos es que posibilitan rotaciones que sólo involucran sumas y desplazamientos de bits (multiplicaciones x2).

·          Una vez reducido el ángulo a un valor suficientemente pequeño (dependiendo de la precisión requerida) se estima su arco tangente aprovechando el hecho que atan(x) es similar a x para x pequeñas.

·          Se suma el ángulo obtenido a los usados en las rotaciones y se obtiene una aproximación de la función arco tangente.

La ventaja de este método (además de no usar multiplicaciones) es que es posible hacer más o menos trabajo en función de la precisión deseada. El siguiente código implementa la función atan2 con un error máximo de 1/64º

// atan2 function using integer arithmetic

// The arguments must be positive and below 512: 0<=(x,y)<=512

// Returns the angle in units of 1/128 degrees

// Works only for the 1st quadrant (0<=angle<=90º)

// Needs additional code to determine the quadrant and return an angle between 0 and 360º

// Maximum error does not exceed 2 units = 2/128 = 1/64 = 0.015º

// Error variance is about 0.78 units = 0.006º

// Within the 0-45º range:  45% error=0, 52% error=1, 2% error=2

uint16 int_atan2(uint16 y, uint16 x)

{

 uint16 angle,y2,t;

 uint16 ang[4]={5760, 3400, 1797, 912};  // atan(1,1/2,1/4,1/8) in 1/128º units

 uint8 k,bbb;

 // Singular cases

 if (y==0) {return 0;    }  // 0º

 if (x==0) {return 11520;}  // 90º

 angle=0;

 if ((y<<3)>x)  //If I'm above atan(1/8) rotate until atan<=1/8  -> x < 8y

  {

   for(k=0;k<4;k++)

    {

     t = (y<<k);

     if (t>=x) { angle+=ang[k]; y2=y;  y = t-x; x = (x<<k)+y2; }

    }

  }

 // Computes integer approximation for y/x.

 // The result is placed in t = 1024 * 8 y/x  y/x=[0 1/8] -> t=[0 1024]

 t=0;

 y <<=4;  // y <- y*16

 for(k=0;k<10;k++)

  {

   t<<=1;

   if (y>=x){y-=x; t++;}

   y<<=1;

  }

 if (y>=x) t++;  // final rounding

 // Apply lineal interpolation to y/x=t :: 1024 -> 912 = 7.125º = atan(1/8)

  t *=57;                                   // 912/1024 aprox. 57/64

  t>>=5;  bbb=t&0x01; t>>=1;  if (bbb) t++; // divide by 64 with rounding

  angle = angle+t+1; // Add the interpolated angle to the previous rotation

  return angle;

}

Algunos comentarios:

• La función asume x ,y >=0 (primer cuadrante). Para implementar la función para cualquier ángulo se precisa añadir código adicional, chequeando los signos de x,y y determinando el cuadrante resultante.
• La precisión de 1/64º es todavía exagerada para nuestra aplicación. A base de no bajar hasta un ángulo tan pequeño podemos construir una función más rápida aunque menos precisa.

La siguiente función tiene un error máximo de 1º y es por lo tanto adecuada para nuestra implementación:

// Rotate angle until angle < atan(1/2)

// Linear interpolation using angle = (y/x)*53 + 0.5

// Works in 1/2º units and divide by 2 before returning the results (in degrees)

uint16 atan2_cordic(uint16 y, uint16 x)

{

 uint16 angle;

 int16 t;

 uint8 b;

 // Singular cases

 if (y==0) return 0; if (x==0) return 90;  // 0º and 90º

 // Rotate until angle is below atan(1/2). Angle in 1/2º units

 angle=0;

 t=y-x;      if(t>=0) {angle=90;  x+=y;         y=t;}

 t=(y<<1)-x; if(t>=0) {angle+=53; x<<=1; x+=y;  y=t;}

   

 y*=106;  y/=x; y++;  // (x/y)*106 + 1

 angle+=y;            // Add previous rotation

 b=angle&0x01; angle>>=1; if (b) angle++;  // divide by 2 with rounding

 

 return angle;

}

Esta gráfica muestra el error (en grados) de esta aproximación para todos los valores de x,y entre 0 y 200:

El s¡guiente código (en brujula2.c) muestra las dos formas alternativas de calcular la función arco tangente, incluyendo también la determinación del cuadrante:

// Using atan2 library function 

     rumbo=atan2((float)acum2,(float)acum1);  rumbo=rumbo*57.2958; //degrees

     rr=(int16)(rumbo);

// Using approximation 

     r1=acum1;  if (r1<0) r1=-r1;

     r2=acum2;  if (r2<0) r2=-r2;

     rr  = atan2_cordic(r2,r1);  // Computes angle in 1^st quad (0,90º) using abs values 

     // Determines quad and modify angle (-180, 180) accordingly   

     if (acum1<0) rr-=180;

     if (acum1*acum2<0) rr=-rr; 

     if ((acum1==0)&&(acum2<0)) rr-=180;

El ahorro tanto en código como en velocidad es considerable. Nuestro nuevo código (función + código para determinar los cuadrantes) ocupa en total menos de 200 palabras, frente a las 2000 de la función de la librería. Un factor x10 en reducción del tamaño del código.

En cuanto a la velocidad, la gráfica adjunta ilustra el tiempo necesario para ejecutar la nueva función (amarillo) frente a la librería (azul, derecha). Unos 0.1 ms frente a los 1.75 ms de antes. Una ganancia de casi un factor 20. En azul, a la izquierda, el pulso más estrecho muestra el tiempo (unos 0.25 ms) usado por la primera función que vimos, con un error máximo de 1/64º. Puede ser una buena opción si queremos mayor precisión, pero sin llegar a la millonésima  de grado que nos ofrece la función de librería.