22 febrero 2015

GPS, cadenas NMEA y máquinas de estados finitos en Arduino

¡Hola a todos!

Tenía ya un tiempo de no escribir, así que decidí que sería bueno compartirles un tema de esos que muchas veces conocemos en teoría pero que muchas veces es difícil visualizar una aplicación en nuestros proyectos.

Antes de comenzar voy a pedirles que se hagan una pregunta: ¿Cómo harían para extraer las posiciones de latitud y longitud de la siguiente cadena en Arduino?:

 $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47

Habiendo hecho esta pregunta... ¡Comencemos!


El estándar NMEA


La cadena corresponde a la salida serial de un GPS que soporta el estándar de comunicación NMEA, en dicho estándar cada "valor" está separado por una coma. Para nuestro ejemplo las partes que realmente nos interesan son '4807.038,N' y '01131.000,E' que corresponden a la latitud y longitud.

Una de las primeras ideas que se te puede venir a la mente es utilizar la función estándar de C "strtok" que sirve para dividir la cadena en función de un carácter separador dado.

Ahora, compliquemos un poco más la pregunta: Asumamos que nuestra cadena no está almacenada en una variable (tipo char*) sino que la estamos leyendo haciendo uso de "Serial.read()" que nos proporciona únicamente un carácter a la vez.

Lo anterior implica que debemos de "almacenar" nuestra cadena en algún lugar antes de poder siquiera comenzar a trabajar en ella. Vamos a utilizar la función propia append_buffer(c) que compartí en una entrada anterior.

Procesando las cadenas


Para refrescarles la memoria, la función append_buffer que creamos se encarga de construir una cadena de caracteres copiando cada carácter nuevo al final de nuestra cadena "buffer" esto lo continua haciendo hasta que nosotros llamamos a clear_buffer o si el buffer se llena en cuyo caso descarta la cadena almacenada en memoria y comienza de nuevo.

Dejemos un poco la teoría y escribamos algo de código:

void loop() {
  if(Serial.available()) {
    char c = Serial.read();
    append_buffer(c);
    // Hacer algo con el buffer
    // ...
  }
}

Hasta este momento no sabemos que tan larga es nuestra cadena, recordemos que estamos procesando caracter por caracter, así que lo más seguro que podemos intentar hacer es "procesarla" cada vez que encontramos un fin de línea. Para evitarnos dolores de cabeza, vamos a "limpiar" nuestro buffer cada vez que encontremos un fin de línea.

En código, lo anterior se vería más o menos como esto:

void loop() {
  if(Serial.available()) {
    char c = Serial.read();
    append_buffer(c);
    if(c=='\n') {
      // Hacer algo con el buffer aquí
      clear_buffer();
    }
  }
}

Ahora estamos listos y vamos a procesar nuestra cadena. Pero ¡Momento!... Resulta que el estándar NMEA no solo tiene cadenas como la que presentamos al principio sino que también contiene otro montón de cadenas extra que no nos interesanpara conocer la posición de nuestro GPS.

Para solventar eso vamos a agregar un pequeño filtro para ejecutar nuestro código de tal manera que se ejecute únicamente cuando nuestra cadena comience con "$GPGGA". Para hacer esto agregamos otra condicional a nuestro código y haremos uso de la función strsrt que retornara NULL (que se procesa como falso) cuando la cadena no es encontrada:

void loop() {
  if(Serial.available()) {
    char c = Serial.read();
    append_buffer(c);
    if(c=='\n') {
      // Hacer algo con el buffer aquí
      if(strstr(buffer,"$GPGGA")) {
 // Encontrada cadena GPGGA, procesar la posición
      }
    }
  }
}

Ahora que tenemos muestro "marco" listo podemos comenzar a trabajar en el código que se encargará de extraer las posiciones. Vamos a usar primero strok que divide la cadena y agregaremos algo de código extra para conocer en que posición de la cadena nos encontramos.

void loop() {
  if(Serial.available()) {
    char c = Serial.read();
    append_buffer(c);
    if(c=='\n') {
      // Hacer algo con el buffer aquí
      if(strstr(buffer,"$GPGGA")) {
        char * pch;
        pch = strtok (str,",");
        int pos = 0; // Con esta variable podremos identificar el segmento
        while (pch != NULL)
        {
          pch = strtok (NULL, " ,.-");
          // Aquí procesamos el texto dependiendo de la posición
          // la latitud estará almacenada en el tercer "token"
          // la orientación Norte o Sur en el cuarto
          // la longitud en el quinto y la orientación Este/Oeste
          // en el sexto. Recordar que la posición comienza en 0
          if(pos==2) {
            // Procesar latitud
          }
          if(pos==3) {
            // Procesar orientación N/S
          }
          if(pos==4) {
            // Procesar longitud
          }
          if(pos==5) {
            // Procesar orientación E/O
          }
        }
      }
    }
  }
}

Hasta aquí todo muy bonito, pero (no se porque tiene que haber siempre un pero). ¿Qué pasa si la cadena esta mal-formada? O si simplemente nuestro GPS no ha logrado capturar una posición. En ese caso podríamos tener una cadena como la siguiente:

$GPGGA,,,,,1,,,,,,,,*47

¡Y vaya sorpresa! nuestro código no va a funcioanr bien porque strok no es capaz de identificar tokens vacíos así que saltará desde la primera coma hasta la última con lo que los datos simplemente se grabarán mal.

Así que tenemos dos opciones. Crear nuestro propio "tokenizador" haciendo uso de strchr o complicar muchísimo más nuestro código para agregar otras condicionales que permitan identificar cadenas mal formadas.

No voy a dar ejemplos de como hacerlo y lo dejo de tarea para el lector, pero si pienso decir algo: No es elegante, es poco eficiente y nos obliga a pensar en todos los errores que puedan existir en el formato de entrada, pero les garantizo que habrá más de algún error que no tomaremos encuenta, porque aceptemoslo de una vez: Murphy es más ingenioso que nosotros.

La máquina de estados finitos


Regresando a nuestro problema: Hagámonos la siguiente pregunta: ¿Qué tal si pudieramos diseñar un algoritmo que no tuviera que esperar a tener la cadena completa sino que la vaya procesando en tanto vaya recibiendo los caracteres? Es decir que fuera observando caracter por caracter e inmediatamente detectara que comienza con $GPGGA pudiera comenzar a procesar los datos que va recibiendo.
 
Hay una construcción teórica de las ciencias de computación que define una máquina de estados finitos (o autómatas de estado finito) como una máquina que varia su estado interno en función de entradas (o estímulos como a mi me gusta llamarle). Normalmente se representa una máquina de estados finitos como circulos unidos por flechas donde cada circulo representa un estado diferente y las flechas indican los estímulos que debe recibir para cambiar de estado.

Hay una restricción: La máquina solo puede encontrarse en un estado a la vez, esto significa también que es totalmente determinística, esto significa que es posible conocer cual  el estado interno de la máquina si conocemos cuales fueron sus entradas.

Para entender como esta construcción nos puede ayudar a procesar la cadena debemos de dejar de ver nuestra cadena NMEA como una cadena y pensar en ella como una secuencia de caracteres.

Digamos por ejemplo que quisieramos detectar la cadena ABBA con una máquina de estados finitos. Muy probablemente tendríamos un diagráma como él siguiente:


La máquina anterior funciona más o menos de la siguiente manera. Primero vamos a asumir que el estado inicial de la máquina es 0, cuando la máquina reciba una letra "a" cambiara su estado inicial a 0, si recibe cualquier otro caracter que no sea "a" (representado por *) regresará al estado inicial. Así para poder llegar hasta el estado 4, que es el estado de aceptación que nos indica que se ha reconocido la cadena ABBA tenemos que hacer que la máquina reciba la secuencia a-b-b-a, cualquier otra secuencia diferente hará que la máquina regrese a su estado inicial.

Asumamos entonces que quisiéramos detectar nuestra cadena abba desde los caracteres que recibimos del puerto Serial de Arduino, nuestro código sería algo similar al siguiente:

int s = 0; // Almacena el estado actual
boolean processed = false; // Nos indica si el código fué procesado o no.
void loop() {
  if(Serial.available()) {
    char c = Serial.read();
    processed = false;
    switch(c) {
      case 'a':
      case 'A':
        if(s==0||s==3) {
          s++;
          if(s==3)
            processed = true;
        }
        else if(s==4) {
          s=1;
          
        else
          s=0;
        break;
              case 'b':
              case 'B':
        if(s==1||s==2)
          s++;
        else
          s=0;
      default:
        s=0;
        break;
    }
    if(processed) {
      // ABBA detectado.
    }
  }
}

El código parece un poco dificil de leer, pero funciona esencialmente de la siguiente manera:
  1. El switch se encarga de comparar el caracter actual y ejecutar el bloque de código correspondiente. Notarán que comparamos por el caracter en minúscula y luego en mayúscula ya que estos tienen valores diferentes.
  2. Dentro de cada case se revisa el estado actual de la máquina y dependiendo de en que estado se encuentre se decide si se cambia al siguiente estado o si se salta a un estado diferente.
  3. El caso por defecto es regresar al estado inicial 0.
Si quisieramos hacer nuestro código más leible, podemos "sacar" nuestra máquina de estados finitos a una función independiente de la siguiente manera:

int s = 0; // Almacena el estado actual

boolean detectar_abba(char c) {
  boolean processed = false;
  switch(c) {
    case 'a':
    case 'A':
      if(s==0||s==3) {
        s++;
        if(s==3)
          processed = true;
      }
      else if(s==4) {
        s=1;
        
      else
        s=0;
      break;
            case 'b':
            case 'B':
      if(s==1||s==2)
        s++;
      else
        s=0;
    default:
      s=0;
      break;
  }
  return processed;
}

void loop() {
  if(Serial.available()) {
    char c = Serial.read();
    if(detectar_abba(c)) {
      // ABBA detectado.
    }
  }
}

El procesador ideal


Aunque la codificación se vuelve un poco más complicada, realmente una máquina de estados finitos nos provee de algunas características que la hacen ideal para procesar nuestra cadena NMEA, voy a enumerar las que considero más importantes:

  1. No requiere de un buffer: A diferencia de la forma "tradicional" de procesar cadenas que utiliza búsquedas de funciones estańdar de C, no necesitamos almacenar la cadena completa y la podemos procesar al vuelo, esto se traduce en un menor uso de memoria, algo que deseamos en entornos limitados como Arduino.
  2. Es a prueba de errores: Si diseñamos correctamente nuestra máquina de estados no tendremos que considerar todos los posibles estados de error (cadenas mal formadas, datos basura) ya que nuestra máquina procesará única y exclusivamente la cadena en el formato que hemos especificado.
  3. El uso de memoria es constante: Solo necesitamos dos variables para que nuestra máquina funcione. Una que almacenará el carácter actual y la otra que guardará el estado interno de la máquina.
  4. No bloquea el ciclo de ejecución: Esto es tal vez la razón principal por la que prefiero utilizar una máquina de estados. Cada vez que la máquina procesa un estado hace únicamente dos comparaciones, la del estado actual y la del caracter de entrada. Ya que no utiliza ningun tipo de ciclos for o while no bloquea el ciclo de ejecución así que podemos utilizar el precioso tiempo de nuestro microcontrolador en cosas más útiles.
Habiendo dicho eso. Diseñemos una máquina de estados finitos que pueda procesar nuestra cadena NMEA. Sin embargo vamos a hacer un par de restricciones:
  1. No vamos a procesar la cadena completa. Para la aplicación que necesitamos únicamente procesaremos la latitud y la longitud, ignorando el resto de la cadena.
  2. No vamos a convertir formatos. Esto es para reducir el numero de ciclos de procesamiento de nuestra máquina simplemente vamos a "extraer" las cadenas de la latitud y longitud en dos cadenas predeterminadas.
Nota: En este diagrama todos los caracteres que no sean los especificados hacen que la máquina regrese al estado 0, sin embargo no se han incluído (a excepción del estado 15) para poder simplificar el diagrama.

Notarán que tenemos varios estados de aceptación. Los estados realizan las siguientes funciones:
  • 9: Almacena la parte entera de la latitud.
  • 10: Almacena la parte decimal de la latitud.
  • 11: Almacena la orientación norte/sur de la latitud.
  • 13: Almacena la parte entera de la longitud.
  • 14: Almacena la parte decimal de la longitud.
  • 15: Almacena la orientación este/oeste de la longitud.
En lugar de convertir la latitud y longitud a un número simplemente lo vamos a extraer así que utilizaremos dos cadenas de un ancho predeterminado para guardar los valores internos. Para guardar los valores simplemente vamos a desplazarnos hacia la derecha en el arreglo correspondiente y vamos a colocar al final de la cadena la orientación.

El código queda entonces de la siguiente manera:

int s = 0;
int p1 = 0;
int p2 = 0;
char latitude[11] = "0000.0000N";
char longitude[12] = "00000.0000W";

boolean nmea_parser(char c) {
  boolean parsed = false;
  switch(c) {
    case '$':
      if(s==0) {
        s++;
        p1=p2=0;
      }
      break;
    case 'g':
    case 'G':
      if(s==1||s==3||s==4)
        s++;
      else
        s=0;
      break;
    case 'p':
    case 'P':
      if(s==2)
        s++;
      else
        s=0;
      break;
    case 'a':
    case 'A':
      if(s==5)
        s++;
      else
        s=0;
      break;
    case ',':
      if(s==6||s==8||s==10||s==12||s==14)
        s++;
      else
        s==0;
      break;
    case '0':
    case '1':
    case '2':
    case '3':
    case '4':
    case '5':
    case '6':
    case '7':
    case '8':
    case '9':
      if(s==9||s==10) {
        if(p1>9) {
          p1=0;
        }
        latitude[p1++] = c;
      } else if(s==13||s==14) {
        if(p2>10) {
          p2=0;
        }
        longitude[p2++] = c;
      }
      break;
    case '.':
      if(s==7||s==9||s==13) {
        if(s==9) {
          latitude[4] = '.';
          p1++;
        }
        if(s==13) {
          longitude[5] = '.';
          p2++;
        }
        s++;
      } else
        s=0;
      break;
    case 's':
    case 'S':
    case 'n':
    case 'N':
      if(s==11) {
        latitude[9] = c;
        s++;
      } else
        s = 0;
      break;
    case 'e':
    case 'E':
    case 'w':
    case 'W':
      if(s==15) {
        longitude[10] = c;
        parsed = true;
      }
      s = 0;
      break;
    default:
      s = 0;
      break;
  }
  return parsed;
}

En nuestro loop principal el código queda de la siguiente manera:

void loop() {
  if(Serial.available()) {
    if(nmea_parser(Serial.read())) {
      Serial.println("Posición NMEA decodificada:");
      Serial.print("Latitude: ");
      Serial.println(latitude);
      Serial.print("Longitude: ");
      Serial.println(longitude);
    }
  }
}

Aunque el código de la máquina de estados es difícil de descifrarse sin ayuda del diagrama de estados nuestro código en el loop principal queda muchísimo más leible.

Pero a este punto, alguien se podría estar preguntando ¿Por qué no utilizar una biblioteca como TinyGPS, sin embargo aunque es muy fácil de usar adolece de varias limitantes que me gustaría listar:

  1. Está basada en C++, esto significa que el código ocupa un poco más de espacio que una solución basada en C.
  2. Utiliza un buffer interno que implica que para procesar las cadenas debe continuamente inicializar y liberar memoria. Para la aplicación en la que trabajo necesitaba un código que utilizara al minimo el espacio de memoria o en su defecto que la memoria utilizara fuera constante.

La Multi-Tarea en Arduino


Un efecto interesante de programar con máquinas de estados finitos es que podemos "emular" multi-tareas en nuestro Arduino. Imaginemos por un momento que las entradas o estimulos no son caracteres del puerto Serial sino el estado de una entrada (que cambia de 0 a 1) o el cambio en una lectura análoga o incluso una interrupción.

Podemos tener múltiples máquinas de estados finitos funcionando en base a estimulos diferentes y como ninguna bloquea el ciclo de ejecución "parecerá" que nuestro Arduino está ejecutando múltiples tareas al mismo tiempo.


Algunas cosas a tomar en consideración


Uno de los grandes problemas de diseñar máquinas de estados es que es muy fácil crear bucles infinitos o estados de los cuales la máquina no puede salir, estos resultan muy difíciles de depurar ya que como vimos la máquina no bloquea el ciclo de ejecución principal.

El código generado como resultado del diseño de la máquina no es fácilmente legible, esto significa que debemos tomarnos suficiente tiempo para revisar si nuestro diseño es correcto y si realmente reacciona de la manera que esperamos. Es buena práctica incluir el diagrama de la máquina de estados junto a nuestro código para facilitar el análisis del código por otros programadores o incluso nosotros mismos.

Un ejemplo completo


El siguiente ejemplo para Arduino Leonardo asume que un GPS con salida NMEA conectado al Serial1, cada vez que detecte una cadena válida imprimirá en la salida las posiciones correspondientes al puerto Serial que corresponde al puerto de comunicaciones con la PC.


Espero que este código sea útil, recuerda agregar tus comentarios al final de esta entrada. No habiendo nada mas que decir: ¡Hasta la próxima!

Recursos útiles:

No hay comentarios: