NORB3.cpp

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
 

char* NMEA_string = "$GPGGA,212748.000,5056.6505,N,00124.3531,W,2,07,1.8,102.1,M,47.6,M,0.8,0000*6B"; // example first part of NMEA sentence
int counter = 0; // sentence id


// function to convert latitude into decimal degrees
int check_latitude(char* latitude, char* ind, float* new_latitude)
{
    float result = 0;

    if (strlen(ind)==1)
    {
	result+=(latitude[0]-'0')*10;
	result+=(latitude[1]-'0');
	result+=(strtod(&latitude[2],NULL) /60);
	
    if (strcmp(ind,"S")==0)
    {
        result*=-1;
    }
	
    *new_latitude = result;
    return 1;
    }
    else
    {
	return -1;
    }
}

// function to convert longitude into decimal degrees
int check_longitude(char* longitude, char* ind, float* new_longitude)
{
	float result = 0;
	
	if (strlen(ind)==1)
	{
	    result+=(longitude[0]-'0')*100;
		result+=(longitude[1]-'0')*10;
		result+=(longitude[2]-'0');
		result+=(strtod(&longitude[3],NULL) /60);
	
	if (strcmp(ind,"W")==0)
	{
		result*=-1;
	}
	
	*new_longitude = result;
    return 1;
	}
	else
	{
		return -1;
	}
}


int parse_NMEA(char* mystring, int flightmode)
{
  float new_latitude;
  float new_longitude;
  char time[12];
  int time_length;
  char identifier[7];
  char latitude[12];
  char north_south[2];
  char longitude[12];
  char east_west[2];
  int lock;
  char satellites[3];
  char altitude[10];
  char datastring[100] = "";
  int check_latitude_error;
  int check_longitude_error;

 
  // split NMEA string into individual data fields and check that a certain number of values have been obtained
  // $GPGGA,212748.000,5056.6505,N,00124.3531,W,2,07,1.8,102.1,M,47.6,M,0.8,0000*6B
  if (sscanf(mystring, "%6[^,],%[^,],%9[^,],%1[^,],%[^,],%1[^,],%d,%2[^,],%*[^,],%[^,]", identifier, time, latitude, north_south, longitude, east_west, &lock, satellites, altitude) == 9)
  {

	  // check for the identifer being invalid
	  if (strncmp(identifier, "$GPGGA", 6) != 0)
	  {
		  printf("Invalid NMEA String...1\n");
	      getchar();
		  return 0;
	  }

	  // check for a valid lock
	  if (lock == 0)
	  {
		  printf("No Lock...\n");
		  getchar();
		  return 0;
	  }

	  time_length = strlen(time); //get length of time
  
      // check time is in the format: HHMMSS unless either already set or NMEA input was invalid
      if (time_length > 6)
      {
           sscanf(time, "%[^.]", time);
      }
      else if (time_length < 6)
      {  
          printf("Invalid NMEA String...2\n");
		  getchar();
		  return 0;
      }

	  // store the return value of the latitude/longitude functions accordinly
	  check_latitude_error = check_latitude(latitude, north_south, &new_latitude);
	  check_longitude_error = check_longitude(longitude, east_west, &new_longitude);

	  // if check_latitude() failed
	  if (check_latitude_error == -1)
	  {
		  printf("Invalid NMEA String...3\n");
		  getchar();
		  return 0;
	  }

	  // if check_longitude() failed
	  if (check_longitude_error == -1)
	  {
		  printf("Invalid NMEA String...4\n");
		  getchar();
		  return 0;
	  }

	  //print data
	  printf("%s\n\n", NMEA_string);
	  printf("%s\n", identifier);
	  printf("%s\n", time);
	  printf("%d\n", counter);
	  printf("%f\n", new_latitude);
	  printf("%s\n", north_south);
	  printf("%f\n", new_longitude);
	  printf("%s\n", east_west);
	  printf("%d\n", lock);
	  printf("%s\n", satellites);
	  printf("%s\n\n", altitude);
	  
	  // pull everything together into a datastring and print
	  sprintf(datastring, "%s,%s,%d,%f,%s,%f,%s,%d,%s,%d,%s", identifier, time, counter, new_latitude, north_south, new_longitude, east_west, lock, satellites, flightmode, altitude);
	  printf(datastring);
	  getchar();
     }
	  else
	 {
		 printf("Invalid NMEA String...5\n");
	     getchar();
		 return 0;
	 }
 
  }


int main(void)
{
    int flightmode = 0; // TODO: replace with proper flightmode setting/checking stuff
	parse_NMEA(NMEA_string, flightmode);
	return 0;
}