source: pacpussensors/trunk/StdDbtPlayerComponents/DbtPlyGgaManager.cpp@ 100

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

//#include "kernel/ComponentManager.h"

namespace pacpus {

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

// double dist1[2];
// double dist2[2];

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;
}





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

}

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

//////////////////////////////////////////////////////////////////////////
// displayData 
//////////////////////////////////////////////////////////////////////////
void DbtPlyGgaManager::processData(road_time_t t, road_timerange_t /*tr*/ , void * buf)
{
    // no data available
    if (buf == NULL) {
        printf("NULL");
        emit displayLat(0);
        emit displayLon(0);
    }
  
  val = (trame_gga_dbl*)(buf);


  emit displayLat(val->lat);
  emit displayLon(val->lon);

}

ComponentBase::COMPONENT_CONFIGURATION DbtPlyGgaManager::configureComponent(XmlComponentConfig config)
{
  DbtPlyFileManager::configureComponent(config);

  return ComponentBase::CONFIGURED_OK;
}




void DbtPlyGgaManager::startActivity()
{ 
  DbtPlyFileManager::startActivity();
  // user interface 
}




void DbtPlyGgaManager::stopActivity()
{
  DbtPlyFileManager::stopActivity();
}


void DbtPlyGgaManager::displayUI()
{
  // todo
        // 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)));
}

} // namespace pacpus