source: Sophya/trunk/Poubelle/archTOI.old/toillgpsproducer.cc@ 556

// toillgpsproducer.cc
// Eric Aubourg         CEA/DAPNIA/SPP   octobre 1999

#include "toillgpsproducer.h"
#include "archfileset.h"
#include "toimanager.h"
#include "requesthandler.h"

#define   gpsTime          "gpsTime"
#define   latitude         "latitude"
#define   longitude        "longitude"
#define   altitude         "altitude"
#define   gpsQuality       "gpsQuality"
#define   gpsNSat          "gpsNSat"
#define   gpsHDOP          "gpsHDOP"
#define   gpsGeoidSep      "gpsGeoidSep"


TOILLGPSProducer::TOILLGPSProducer()
{
  possibleTOIs.insert(TOI(gpsTime,      TOI::unspec,   "", "seconds", "onboardGPS"));  
  possibleTOIs.insert(TOI(latitude,     TOI::unspec,   "", "degrees", "onboardGPS"));  // + = NORTH
  possibleTOIs.insert(TOI(longitude,    TOI::unspec,   "", "degrees", "onboardGPS"));  // + = EAST
  possibleTOIs.insert(TOI(altitude,     TOI::unspec,   "", "meters",  "onboardGPS"));  
  possibleTOIs.insert(TOI(gpsQuality,   TOI::unspec,   "", "flag",    "onboardGPS"));  
  possibleTOIs.insert(TOI(gpsNSat,      TOI::unspec,   "", "integer", "onboardGPS"));  
  possibleTOIs.insert(TOI(gpsHDOP,      TOI::unspec,   "", "real",    "onboardGPS"));  
  possibleTOIs.insert(TOI(gpsGeoidSep,  TOI::unspec,   "", "meters",  "onboardGPS"));
}

string TOILLGPSProducer::getName() {
  return("TOILLGPSProducer 1.0");
}


// 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,,


void TOILLGPSProducer::handleBlock(ArchFileSet* fs)
{
  block_type_gps* blk = fs->lastGPS();
  long sample0 = numero_block(blk)*72;
    
  double gpsutc;        //  secondes depuis minuit
  double gpslat;        //  degres, + = NORD
  double gpslong;       //  degres, + = EST
  double alt;           //  altitude en metres
  int    quality;       //  0 = NO FIX, 1 = GPS FIX, 2 = DGPS FIX
  int    nsat;          //  nombre de satellites utilises
  double hdop;          //  horizontal dilution of precision
  double gsep;          //  separation geoide en metres
  
  char* p = blk->gps;
  char* fence = p+80;
  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') && *p != '-') return;
  sscanf(p, "%lg", &gsep);
  while (*p != ',' && p<fence) p++;
  if (*p != ',') return; 
  p++;
  if (*p != 'M') return; 

  for (set<TOI>::iterator i = producedTOIs.begin(); i != producedTOIs.end(); i++) {
    if ((*i).name == gpsTime)        computedValue((*i), sample0, gpsutc);
    if ((*i).name == latitude)       computedValue((*i), sample0, gpslat);
    if ((*i).name == longitude)      computedValue((*i), sample0, gpslong);
    if ((*i).name == altitude)       computedValue((*i), sample0, alt);
    if ((*i).name == gpsQuality)     computedValue((*i), sample0, quality);
    if ((*i).name == gpsNSat)        computedValue((*i), sample0, nsat);
    if ((*i).name == gpsHDOP)        computedValue((*i), sample0, hdop);
    if ((*i).name == gpsGeoidSep)    computedValue((*i), sample0, gsep);
  }
}