source: pacpussensors/trunk/StdDbtPlayerComponents/DbtPlyGgaManager.cpp

/*********************************************************************
// created:    2007/04/12 - 16:30

//
// author:     Elie Al Alam & Gerald Dherbomez
// 
// version:    $Id: DbtPlyGgaManager.cpp 1203 2012-08-02 11:58:15Z morasjul $
//
// purpose:    Dbite Player GGA Manager implementation
*********************************************************************/

#include "DbtPlyGgaManager.h"

#include <cmath>
#include <qapplication.h>

using namespace pacpus;
using namespace std;

/************************************************************************
 * Construction de la fabrique de composant DbtPlyGgaManager
 ************************************************************************/
static ComponentFactory<DbtPlyGgaManager> sFactory("DbtPlyGgaManager"); 


/************************************************************************
 * Constructor
 ************************************************************************/
DbtPlyGgaManager::DbtPlyGgaManager(QString name)
    : DbtPlyFileManager(name)
{

}


/************************************************************************
 * Destructor
 ************************************************************************/
DbtPlyGgaManager::~DbtPlyGgaManager()
{
        
}


/************************************************************************
 * Process GGA data
 ************************************************************************/
void DbtPlyGgaManager::processData(road_time_t t, road_timerange_t tr , void * buffer)
{
    // no data available
    if (!buffer) {
        LOG_DEBUG("no data available: NULL buffer");
        return;
    }

    // make local copy of GGA frame
    memcpy(&mGga.frame, buffer, sizeof(trame_gga_dbl));
    mGga.time = t;
    mGga.timerange = tr;

    // send GGA data to output
    checkedSend(outGga, mGga);
}


/************************************************************************
 * Start function, called by the ComponentManager when a start()
 * command is received
 ************************************************************************/
void DbtPlyGgaManager::startActivity()
{ 
  DbtPlyFileManager::startActivity();

  outGga = getTypedOutput<TimestampedGgaFrame, DbtPlyGgaManager>("gga");
  // user interface 
}


/************************************************************************
 * Stop function, called by the ComponentManager when a stop()
 * command is received
 ************************************************************************/
void DbtPlyGgaManager::stopActivity()
{
  DbtPlyFileManager::stopActivity();
}


/************************************************************************
 * Called by the framework at initialization
 ************************************************************************/
void DbtPlyGgaManager::addInputs()
{
  // uncomment to add an input
}


/************************************************************************
 * Called by the framework at initialization
 ************************************************************************/
void DbtPlyGgaManager::addOutputs()
{
  // empty: no output
  addOutput<TimestampedGgaFrame, DbtPlyGgaManager>("gga");
}


/************************************************************************
 * Graphical user interface
 ************************************************************************/

/*
double atanh(double  z)
{
    return (0.5 * log((z+1)/(1-z)));
}

double  abs_me(double x)
{
    if(x<0) x=-x;
    return x;
}

//////////////////////////////////////////////////////////////////////////
//transformation des longitudes tatitudes en Lambert93
//////////////////////////////////////////////////////////////////////////
void lonlattolam(double lon, double lat, double & lam93x, double & lam93y)
{
  double GRS_a = 6378137;
  double GRS_f = 1/298.257222101;

  double GRS_b = GRS_a*(1-GRS_f);
  double GRS_bb= GRS_b*GRS_b;
  double GRS_aa= 40680631590769.0;
  double GRS_e = sqrt((GRS_aa - GRS_bb) / (GRS_aa));

  double n = 0.725607765053267;
  double C = 11754255.4261;
  double XS = 700000;
  double YS = 12655612.0499;

  double latiso;
  latiso = atanh(sin(lat)) - GRS_e*atanh(GRS_e*sin(lat));
  double gamma;
  gamma = (lon - 0.0523598775598299)*n;
  double R;
  R = C * exp(-n*latiso);

  lam93x = R *sin(gamma)+XS;
  lam93y = -R *cos(gamma)+YS;
}

//////////////////////////////////////////////////////////////////////////
//Transformation des coordonnees de Lambert93 en Longitude lattitude
//////////////////////////////////////////////////////////////////////////
void lamtolonlat(double lamx, double lamy, double & lon, double & lat)
{
    double GRS_a = 6378137;
    double GRS_f = 1/298.257222101;
    double GRS_b = GRS_a*(1-GRS_f);
    double GRS_bb= GRS_b*GRS_b;
    double GRS_aa= 40680631590769.0;
    double GRS_e = sqrt((GRS_aa - GRS_bb) / (GRS_aa));

    //double n = 0.725607765053267;
    //double C = 11754255.4261;
    //double XS = 700000;
    //double YS = 12655612.0499;


    //lamx = lamx-700000;
    //lamy = lamy-12655612.0499;

    double gamma;
    gamma = atan(-(lamx-700000)/(lamy-12655612.0499));


    lon = gamma/0.725607765053267 + 0.0523598775598299;
    double R;
    R = sqrt((lamx-700000) * (lamx-700000) + (lamy-12655612.0499) * (lamy-12655612.0499));

    double latiso;
    latiso = log((11754255.4261)/R)/(0.725607765053267);

    double phiNew, phiOld;
    phiOld =1;
    phiNew= asin (tanh ( latiso + GRS_e * atanh(GRS_e * sin(phiOld))));
    //printf("\nphiNew: %.20lf",phiNew);
    while (abs_me(phiOld-phiNew) > 1e-10)
    {

        if(abs_me(phiOld-phiNew) > 1e-10)
        {

            phiOld = phiNew;
            phiNew = asin(tanh(latiso+GRS_e*atanh(GRS_e*sin(phiOld))));
        }
        else
            phiOld = phiNew;
    }

    lat = phiNew;
}*/

void DbtPlyGgaManager::displayUI()
{
    // TODO
    LOG_WARN("GUI not implemented");

        // afficher lat lon
        // voir dans DbtPlyUserInterface§.cpp
    /*w = new QWidget();
        w->setWindowTitle(name());
        w->show();
        w->setFixedSize (170,80);
        
        prop = new QGroupBox ("GGA Properties",w);
        prop->show();
        prop->setFixedSize(170,78);
  
        lat = new QLabel("Latitude ", prop);
        lat->move( 5, 15);
        lat->show();
        lat->setFixedSize (80,20);
        latVal = new QLCDNumber (prop);
        latVal->move( 5, 35);
        latVal->setFixedSize(50,20);
        latVal->show();
        connect(this, SIGNAL(displayLat(double)) , latVal , SLOT(display(double)));
  
        lon = new QLabel("Longitude ", prop);
        lon->move( 90, 15);
        lon->show();
        lon->setFixedSize (80,27);
  
        lonVal = new QLCDNumber (prop);
        lonVal->move( 90, 35);
        lonVal->setFixedSize(50,20);
        lonVal->show();
    connect (this, SIGNAL(displayLon(double)) , lonVal , SLOT(display(double)));*/
}


/************************************************************************
 * Configuration of the component, called by the ComponentManager after
 * the construction of the object
 ************************************************************************/
ComponentBase::COMPONENT_CONFIGURATION DbtPlyGgaManager::configureComponent(XmlComponentConfig config)
{
  DbtPlyFileManager::configureComponent(config);

  return ComponentBase::CONFIGURED_OK;
}