C Gps nmea parser from linux serial port does not

问题:

I need to create a c gps nmea parser for a board I'm working on (Cubietruck with armbian debian jessie 8.0) . Based on several examples I found on the internet I concluded in the following:

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <string.h>

int fd = -1;
int end_of_loop= 0;

void sig_handler(int sig)
{
  if(sig == SIGINT)
  {
    printf("GPS parsing stopped by SIGINT\n");
    end_of_loop = 1;
    close(fd);
  }
}



int main(int argc, char *argv[])
{
  struct termios newt;
  char *nmea_line;
  char *parser;
  double latitude;
  float longitude;
  float altitude;

  signal(SIGINT, sig_handler);

  fd = open("/dev/ttyACM2", O_RDWR | O_NONBLOCK);
  if (fd >= 0)
  {
    tcgetattr(fd, &newt);
    newt.c_iflag &= ~IGNBRK;         
    newt.c_iflag &= ~(IXON | IXOFF | IXANY); 
    newt.c_oflag = 0;               

    newt.c_cflag |= (CLOCAL | CREAD);               
    newt.c_cflag |= CS8;                       
    newt.c_cflag &= ~(PARENB | PARODD);         
    newt.c_cflag &= ~CSTOPB;                   

    newt.c_lflag = 0;                            

    newt.c_cc[VMIN]  = 0; 
    newt.c_cc[VTIME] = 0; 
    tcsetattr(fd, TCSANOW, &newt);



  usleep(100000);

  while(end_of_loop == 0)
  {

    char read_buffer[1000];
    read(fd, &read_buffer,1000);
    //printf("|%s|", r_buf);

    nmea_line = strtok(read_buffer, "\n");

    while (nmea_line != NULL)
    {

      parser = strstr(nmea_line, "$GPRMC");
      if (parser != NULL)
      {
        printf("|%s| \n", nmea_line);
        char *token = strtok(nmea_line, ",");
        int index = 0;
        while (token != NULL)
        {
          if (index == 3)
          {
            latitude = atof(token);
            printf("found latitude: %s %f\n", token, latitude);
          }
          if (index == 5)
          {
            longitude = atof(token);
            printf("found longitude: %s %f\n", token, longitude);
          }
          token = strtok(NULL, ",");
          index++;
        }
      }

      parser = strstr(nmea_line, "$GPGGA");
      if (parser != NULL)
      {
        printf("|%s| \n", nmea_line);
        char *token = strtok(nmea_line, ",");
        int index = 0;
        while (token != NULL)
        {
          if (index == 13)
          {
            altitude = atof(token);
            printf("found altitude: %s %f\n", token, altitude);
          }
          token = strtok(NULL, ",");
          index++;
        }

      }



      printf("|%s| \n", nmea_line);
      nmea_line = strtok(NULL, "\n");
    }

    usleep(500000);

  }

  close(fd);

  return 0;

  }
  else
  {
    printf("Port cannot be opened");
    return -1;
  }
}

For the time being I test negative case where there is no GPS fix. The output from the serial port for that case is for every read :

$GNGNS,,,,,,NNN,,,,,,*1D
$GPVTG,,T,,M,,N,,K,N*2C
$GPGSA,A,1,,,,,,,,,,,,,,,*1E
$GNGSA,A,1,,,,,,,,,,,,,,,*00
$GPGGA,,,,,,0,,,,,,,,*66
$GPRMC,,V,,,,,,,,,,N*53

When I run the code I get parse print outs for the GPGGA but not for the GPRMC:

GNGNS,,,,,,NNN,,,,,,*1D
| GPVTG,,T,,M,,N,,K,N*2C
| GPGSA,A,1,,,,,,,,,,,,,,,*1E
| GNGSA,A,1,,,,,,,,,,,,,,,*00
| GPGGA,,,,,,0,,,,,,,,*66
|$GPGGA|
| GNGNS,,,,,,NNN,,,,,,*1D
| GNGNS,,,,,,NNN,,,,,,*1D
| GPVTG,,T,,M,,N,,K,N*2C
| GPGSA,A,1,,,,,,,,,,,,,,,*1E
| GNGSA,A,1,,,,,,,,,,,,,,,*00
| GPGGA,,,,,,0,,,,,,,,*66
|$GPGGA|

I assume that has something to do with the fact that GPRMC is on the last line and when nmea_line = strtok(NULL, "\n"); is executed then the nmea_lime becomes NULL. I added a dummy line on the read_buffer with strcat but with no success.
I printed the index and I found that for GPGGA only index = 3 is achieved. I increased the usleep time but there was no change. Has anyone any idea what I can do to achieve correct parsing?

回答1:

Your idea of parsing seems OK, but the implementation has a few problems though.

I wrote many years ago a gps nmea parser, and as far as I can remember, the lines ended with "\r\n", that would seem also the case because for this line

printf("|%s| \n", nmea_line);

you get

| GPVTG,,T,,M,,N,,K,N*2C

If you change it to

printf(">%s| \n", nmea_line);

you'll most probably see

< GNGNS,,,,,,NNN,,,,,,*1D

A second problem is that you are using strtok in a reentrant kind of way. At the beginning of the loop, you do nmea_line = strtok(read_buffer, "\n");. Then you go to parse the line and enter a new loop. Then you do stuff line char *token = strtok(nmea_line, ","); and by doing this strtok forgets the information about the first call.

At the end of everything you do again nmea_line = strtok(NULL, "\n");, but this NULL applies to which strtok? Depending on the output you will never know but it won't certainly match to with to nmea_line = strtok(read_buffer, "\n");.

Luckly there is a reentrant version of strtok: strtok_r

man strtok

#include <string.h>

char *strtok(char *str, const char *delim);

char *strtok_r(char *str, const char *delim, char **saveptr);

DESCRIPTION

The strtok() function breaks a string into a sequence of zero or more nonempty tokens. On the first call to strtok(), the string to be parsed should be specified in str. In each subsequent call that should parse the same string, str must be NULL.

[...]

The strtok_r() function is a reentrant version strtok(). The saveptr argument is a pointer to a char* variable that is used internally by strtok_r() in order to maintain context between successive calls that parse the same string.

Example:

char line[] = "a:b:c,d:e:f,x:y:z";

char *s1, *s2, *token1, *token2, *in1, *in2;

in1 = line;

while(token1 = strtok_r(in1, ",", &s1))
{
    in1 = NULL; // for subsequent calls

    in2 = token1;

    printf("First block: %s\n", token1);

    while(token2 = strtok_r(in2, ":", &s2))
    {
        in2 = NULL; // for subsequent calls

        printf("  val: %s\n", token2);
    }
}

Output:

First block: a:b:c
  val: a
  val: b
  val: c
First block: d:e:f
  val: d
  val: e
  val: f
First block: x:y:z
  val: x
  val: y
  val: z

Another problem that I see is this:

while(...)
{
    read(fd, &read_buffer,1000);

    nmea_line = strtok(read_buffer, "\n");
}

The read function, unlike fgets, does not read strings, it reads bytes. That means that read doesn't care what it's reading. If the sequence happens to be a sequence of values that match the values of the ASCII table, it won't add the '\0'-terminating byte in your read buffer. And that's a problem, because you are using functions that expect valid strings. If the read input does not contain a newline, strtok will continue reading until it finds '\0' and if that byte is not present, it will read beyond the limits. This is undefined behaviour.

The second problem with doing this is that once again read doesn't care about the bytes you are ready, you are not reading lines, you are ready a 1000 byte block of memory which may or might not contain strings. It is most likely that the block does not contain strings, because /dev/ttyACM2 will generate an endless stream and never send '\0' to the user.

I would use fgets to get a line and parse it, after that get another line, and so on. Because you've only got the file descriptor, you should use:

fdopen

#include <stdio.h>

FILE *fdopen(int fd, const char *mode);

The fdopen() function associates a stream with the existing file descriptor, fd. The mode of the stream (one of the values "r", "r+", "w", "w+", "a", "a+") must be compatible with the mode of the file descriptor. The file position indicator of the new stream is set to that belonging to fd, and the error and end-of-file indicators are cleared. Modes "w" or "w+" do not cause truncation of the file. The file descriptor is not dup'ed, and will be closed when the stream created by fdopen() is closed. The result of applying fdopen() to a shared memory object is undefined.

So I would do:

FILE *f = fopen(fd, "r");


// the gps nmea lines are never that long
char line[64];

char *t1_save;

while(fgets(line, sizeof line, f))
{
    // get rid of \r\n
    line[strcspn(line, "\r\n")] = 0;

    parser = strstr(line, "$GPRMC");
    if(parser)
    {
        // do the parsing
    }

    ...
}

In this version you wouldn't even need strtok_r because you wouldn't need to nest strtok calls.

---

edit

There is one thing that I've missed earlier:

int end_of_loop= 0;

void sig_handler(int sig)
{
  if(sig == SIGINT)
  {
    printf("GPS parsing stopped by SIGINT\n");
    end_of_loop = 1;
    close(fd);
  }
}

int main(int argc, char *argv[])
{
    ...
    while(end_of_loop == 0)
    {
    }
}

Depending in the optimazations of your compiler, you will end up in an endless loop, even after pressing Ctrl+C. The compiler might optimize the while loop to while(1), because in main the end_of_loop variable is never altered, so there is no point to always check for that value.

When trying to stop loops with catching signals, it's best to at least declare the variable as volatile, so that the compiler does not optimize that variable away. Mostly (see 1, 2) the best way to do this is:

volatile sig_atomic_t end_of_loop= 0;

void sig_handler(int sig)
{
  if(sig == SIGINT)
  {
    printf("GPS parsing stopped by SIGINT\n");
    end_of_loop = 1;
    close(fd);
  }
}