source: Sophya/trunk/Poubelle/archTOI.old/gpsparser.cc@ 3736

// gpsparser.cc
// Eric Aubourg         CEA/DAPNIA/SPP   juillet 1999


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

#include "gpsparser.h"

GPSParser::GPSParser() {
  *gpsString = 0;
  stringOk = false;
  gpsutc = 0;
  gpslat = 0;
  gpslong = 0;
  quality = 0;
  nsat = 0;
  hdop = 0;
  alt = 0;
  gsep = 0;
}

GPSParser::~GPSParser() {
}

bool GPSParser::hasTime() {
  return (stringOk && gpsutc>0);
}

bool GPSParser::hasGPSTime() {
  return (stringOk && gpsutc>0 && quality>0 && nsat>=3);
}

bool GPSParser::hasPosition() {
  return (stringOk && quality>0 && nsat>=3);
}

bool GPSParser::hasAltitude() {
  return (stringOk && quality>0 && nsat>=4);
}

double GPSParser::getUTC() {
  if (!stringOk) return -99999;
  return gpsutc;
}

double GPSParser::getLatitude() {
  if (!stringOk) return -99999;
  if (quality == 0 || nsat<3) return -99999;
  return gpslat;
}

double GPSParser::getLongitude() {
  if (!stringOk) return -99999;
  if (quality == 0 || nsat<3) return -99999;
  return gpslong;
}

double GPSParser::getAltitude() {
  if (!stringOk) return -99999;
  if (quality == 0 || nsat<4) return -99999;
  return alt;
}

// GPGGA string : 
// $hhmmss.ss,ddmm.mmmm,n,dddmm.mmmm,e,q,ss,y.y,a.a,z,,
// $104020,3740.712,N,00400.781,W,0,00,01.0,0003,M,-050,M,,

#include <fstream.h>
#include <iomanip.h>
//ofstream gpsdump("gpsdump");
//ofstream gpsdata("gpsdata");
//double outc = 0; double jr = 0;

void GPSParser::ProcessBlock(block_type_gps* blk) {
  strncpy(gpsString, blk->gps, 80);
  char* p = gpsString;
//  gpsdump << p ; if (p[strlen(p)-1] != '\n') gpsdump << '\n';
  char* fence = p+80;
  stringOk = false;
  if (*p!='$') return;
  p++;
  
  // UTC
  if (*p < '0' || *p > '9') return;
  double t;
  sscanf(p, "%lg", &t);
  int h = int(t/10000); t -= h*10000;
  int m = int(t/100);   t -= m*100;
  gpsutc = h*3600. + m*60. + t;
  p += 6;
  if (*p != ',') return;
  p++;
  if (p>=fence) return;
  
  // LATITUDE
  if (*p < '0' || *p > '9') return;
  sscanf(p, "%lg", &t);
  int d = int(t/100); t -= d*100;
  t = d + t/60;
  p += 8;
  if (*p != ',') return; 
  p++;
  if (*p == 'S') t = -t;
  gpslat = t;
  p++;
  if (*p != ',') return; 
  p++;
  if (p>=fence) return;

  // LONGITUDE
  if (*p < '0' || *p > '9') return;
  sscanf(p, "%lg", &t);
  d = int(t/100); t -= d*100;
  t = d + t/60;
  p += 9;
  if (*p != ',') return; 
  p++;
  if (*p == 'W') t = -t;
  gpslong = t;
  p++;
  if (*p != ',') return; 
  p++;
  if (p>=fence) return;

  // QUALITY
  quality = *p - '0';
  if (quality < 0 || quality > 9) return;
  p++;
  if (*p != ',') return; 
  p++;
  if (p>=fence) return;
  
  // NSAT
  if (*p < '0' || *p > '9') return;
  sscanf(p, "%d", &nsat);
  p += 2;
  if (*p != ',') return; 
  p++;
  
  // HDOP
  if (*p < '0' || *p > '9') return;
  sscanf(p, "%lg", &hdop);
  while (*p != ',' && p<fence) p++;
  if (*p != ',') return; 
  p++;
  
  // Altitude
  if (*p < '0' || *p > '9') return;
  sscanf(p, "%lg", &alt);
  while (*p != ',' && p<fence) p++;
  if (*p != ',') return; 
  p++;
  if (*p != 'M') return; 
  p++;
  if (*p != ',') return; 
  p++;

  // Geoid separation
  if (*p < '0' || *p > '9') return;
  sscanf(p, "%lg", &gsep);
  while (*p != ',' && p<fence) p++;
  if (*p != ',') return; 
  p++;
  if (*p != 'M') return; 

//  if (gpsutc < outc) jr += 86400; 
//  outc = gpsutc;
//  gpsdata << setprecision(10) << gpsutc + jr << " " << gpslat << " " << gpslong << " " << quality << " " << nsat << '\n';

  stringOk = true;
}