Changeset 103 in pacpussensors for trunk


Ignore:
Timestamp:
Nov 22, 2015, 4:54:36 PM (9 years ago)
Author:
xuphilip
Message:

I/O update

Location:
trunk/StdDbtPlayerComponents
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • trunk/StdDbtPlayerComponents/DbtPlyGgaManager.cpp

    r82 r103  
    1515#include <qapplication.h>
    1616
    17 //#include "kernel/ComponentManager.h"
    18 
    19 namespace pacpus {
    20 
    21 //////////////////////////////////////////////////////////////////////////
    22 // Construction de la fabrique de composant DbtPlyGgaManager
    23 //////////////////////////////////////////////////////////////////////////
     17using namespace pacpus;
     18using namespace std;
     19
     20/************************************************************************
     21 * Construction de la fabrique de composant DbtPlyGgaManager
     22 ************************************************************************/
    2423static ComponentFactory<DbtPlyGgaManager> sFactory("DbtPlyGgaManager");
    2524
    26 // double dist1[2];
    27 // double dist2[2];
    28 
     25
     26/************************************************************************
     27 * Constructor
     28 ************************************************************************/
     29DbtPlyGgaManager::DbtPlyGgaManager(QString name)
     30    : DbtPlyFileManager(name)
     31{
     32
     33}
     34
     35
     36/************************************************************************
     37 * Destructor
     38 ************************************************************************/
     39DbtPlyGgaManager::~DbtPlyGgaManager()
     40{
     41       
     42}
     43
     44
     45/************************************************************************
     46 * Process GGA data
     47 ************************************************************************/
     48void DbtPlyGgaManager::processData(road_time_t t, road_timerange_t tr , void * buffer)
     49{
     50    // no data available
     51    if (!buffer) {
     52        LOG_DEBUG("no data available: NULL buffer");
     53        return;
     54    }
     55
     56    // make local copy of GGA frame
     57    memcpy(&mGga.frame, buffer, sizeof(trame_gga_dbl));
     58    mGga.time = t;
     59    mGga.timerange = tr;
     60
     61    // send GGA data to output
     62    checkedSend(outGga, mGga);
     63}
     64
     65
     66/************************************************************************
     67 * Start function, called by the ComponentManager when a start()
     68 * command is received
     69 ************************************************************************/
     70void DbtPlyGgaManager::startActivity()
     71{
     72  DbtPlyFileManager::startActivity();
     73
     74  outGga = getTypedOutput<TimestampedGgaFrame, DbtPlyGgaManager>("gga");
     75  // user interface
     76}
     77
     78
     79/************************************************************************
     80 * Stop function, called by the ComponentManager when a stop()
     81 * command is received
     82 ************************************************************************/
     83void DbtPlyGgaManager::stopActivity()
     84{
     85  DbtPlyFileManager::stopActivity();
     86}
     87
     88
     89/************************************************************************
     90 * Called by the framework at initialization
     91 ************************************************************************/
     92void DbtPlyGgaManager::addInputs()
     93{
     94  // uncomment to add an input
     95}
     96
     97
     98/************************************************************************
     99 * Called by the framework at initialization
     100 ************************************************************************/
     101void DbtPlyGgaManager::addOutputs()
     102{
     103  // empty: no output
     104  addOutput<TimestampedGgaFrame, DbtPlyGgaManager>("gga");
     105}
     106
     107
     108/************************************************************************
     109 * Graphical user interface
     110 ************************************************************************/
     111
     112/*
    29113double atanh(double  z)
    30114{
    31         return (0.5 * log((z+1)/(1-z)));
     115    return (0.5 * log((z+1)/(1-z)));
    32116}
    33117
    34118double  abs_me(double x)
    35119{
    36         if(x<0) x=-x;
    37         return x;
    38 }
    39 
    40 
    41 
     120    if(x<0) x=-x;
     121    return x;
     122}
    42123
    43124//////////////////////////////////////////////////////////////////////////
    … …  
    48129  double GRS_a = 6378137;
    49130  double GRS_f = 1/298.257222101;
    50  
     131
    51132  double GRS_b = GRS_a*(1-GRS_f);
    52133  double GRS_bb= GRS_b*GRS_b;
    53134  double GRS_aa= 40680631590769.0;
    54135  double GRS_e = sqrt((GRS_aa - GRS_bb) / (GRS_aa));
    55  
     136
    56137  double n = 0.725607765053267;
    57138  double C = 11754255.4261;
    58139  double XS = 700000;
    59140  double YS = 12655612.0499;
    60  
     141
    61142  double latiso;
    62143  latiso = atanh(sin(lat)) - GRS_e*atanh(GRS_e*sin(lat));
    … …  
    65146  double R;
    66147  R = C * exp(-n*latiso);
    67  
     148
    68149  lam93x = R *sin(gamma)+XS;
    69150  lam93y = -R *cos(gamma)+YS;
    70151}
    71152
    72 
    73 
    74 
    75 
    76153//////////////////////////////////////////////////////////////////////////
    77154//Transformation des coordonnees de Lambert93 en Longitude lattitude
    … …  
    79156void lamtolonlat(double lamx, double lamy, double & lon, double & lat)
    80157{
    81         double GRS_a = 6378137;
    82         double GRS_f = 1/298.257222101;
    83         double GRS_b = GRS_a*(1-GRS_f);
    84         double GRS_bb= GRS_b*GRS_b;
    85         double GRS_aa= 40680631590769.0;
    86         double GRS_e = sqrt((GRS_aa - GRS_bb) / (GRS_aa));
    87 
    88         //double n = 0.725607765053267;
    89         //double C = 11754255.4261;
    90         //double XS = 700000;
    91         //double YS = 12655612.0499;
    92 
    93        
    94         //lamx = lamx-700000;
    95         //lamy = lamy-12655612.0499;
    96 
    97         double gamma;
    98         gamma = atan(-(lamx-700000)/(lamy-12655612.0499));
    99 
    100 
    101         lon = gamma/0.725607765053267 + 0.0523598775598299;
    102         double R;
    103         R = sqrt((lamx-700000) * (lamx-700000) + (lamy-12655612.0499) * (lamy-12655612.0499));
    104 
    105         double latiso;
    106         latiso = log((11754255.4261)/R)/(0.725607765053267);
    107 
    108         double phiNew, phiOld;
    109         phiOld =1;
    110         phiNew= asin (tanh ( latiso + GRS_e * atanh(GRS_e * sin(phiOld))));
    111         //printf("\nphiNew: %.20lf",phiNew);
    112         while (abs_me(phiOld-phiNew) > 1e-10)
    113         {
    114                
    115                 if(abs_me(phiOld-phiNew) > 1e-10)
    116                 {
    117        
    118                         phiOld = phiNew;
    119                         phiNew = asin(tanh(latiso+GRS_e*atanh(GRS_e*sin(phiOld))));
    120                 }
    121                 else
    122                         phiOld = phiNew;
    123         }
    124        
    125         lat = phiNew;
    126 }
    127 
    128 
    129 
    130 
    131 
    132 //////////////////////////////////////////////////////////////////////////
    133 /// Constructor
    134 DbtPlyGgaManager::DbtPlyGgaManager(QString name)
    135     : DbtPlyFileManager(name)
    136 {
    137 
    138 }
    139 
    140 //////////////////////////////////////////////////////////////////////////
    141 /// Destructor
    142 DbtPlyGgaManager::~DbtPlyGgaManager()
    143 {
    144        
    145 }
    146 
    147 //////////////////////////////////////////////////////////////////////////
    148 // displayData
    149 //////////////////////////////////////////////////////////////////////////
    150 void DbtPlyGgaManager::processData(road_time_t t, road_timerange_t /*tr*/ , void * buf)
    151 {
    152     // no data available
    153     if (buf == NULL) {
    154         printf("NULL");
    155         emit displayLat(0);
    156         emit displayLon(0);
     158    double GRS_a = 6378137;
     159    double GRS_f = 1/298.257222101;
     160    double GRS_b = GRS_a*(1-GRS_f);
     161    double GRS_bb= GRS_b*GRS_b;
     162    double GRS_aa= 40680631590769.0;
     163    double GRS_e = sqrt((GRS_aa - GRS_bb) / (GRS_aa));
     164
     165    //double n = 0.725607765053267;
     166    //double C = 11754255.4261;
     167    //double XS = 700000;
     168    //double YS = 12655612.0499;
     169
     170
     171    //lamx = lamx-700000;
     172    //lamy = lamy-12655612.0499;
     173
     174    double gamma;
     175    gamma = atan(-(lamx-700000)/(lamy-12655612.0499));
     176
     177
     178    lon = gamma/0.725607765053267 + 0.0523598775598299;
     179    double R;
     180    R = sqrt((lamx-700000) * (lamx-700000) + (lamy-12655612.0499) * (lamy-12655612.0499));
     181
     182    double latiso;
     183    latiso = log((11754255.4261)/R)/(0.725607765053267);
     184
     185    double phiNew, phiOld;
     186    phiOld =1;
     187    phiNew= asin (tanh ( latiso + GRS_e * atanh(GRS_e * sin(phiOld))));
     188    //printf("\nphiNew: %.20lf",phiNew);
     189    while (abs_me(phiOld-phiNew) > 1e-10)
     190    {
     191
     192        if(abs_me(phiOld-phiNew) > 1e-10)
     193        {
     194
     195            phiOld = phiNew;
     196            phiNew = asin(tanh(latiso+GRS_e*atanh(GRS_e*sin(phiOld))));
     197        }
     198        else
     199            phiOld = phiNew;
    157200    }
    158  
    159   val = (trame_gga_dbl*)(buf);
    160 
    161 
    162   emit displayLat(val->lat);
    163   emit displayLon(val->lon);
    164 
    165 }
    166 
    167 ComponentBase::COMPONENT_CONFIGURATION DbtPlyGgaManager::configureComponent(XmlComponentConfig config)
    168 {
    169   DbtPlyFileManager::configureComponent(config);
    170 
    171   return ComponentBase::CONFIGURED_OK;
    172 }
    173 
    174 
    175 
    176 
    177 void DbtPlyGgaManager::startActivity()
    178 {
    179   DbtPlyFileManager::startActivity();
    180   // user interface
    181 }
    182 
    183 
    184 
    185 
    186 void DbtPlyGgaManager::stopActivity()
    187 {
    188   DbtPlyFileManager::stopActivity();
    189 }
    190 
     201
     202    lat = phiNew;
     203}*/
    191204
    192205void DbtPlyGgaManager::displayUI()
    193206{
    194   // todo
     207    // TODO
     208    LOG_WARN("GUI not implemented");
     209
    195210        // afficher lat lon
    196211        // voir dans DbtPlyUserInterface§.cpp
    197         w = new QWidget();
     212    /*w = new QWidget();
    198213        w->setWindowTitle(name());
    199214        w->show();
    … …  
    223238        lonVal->setFixedSize(50,20);
    224239        lonVal->show();
    225         connect (this, SIGNAL(displayLon(double)) , lonVal , SLOT(display(double)));
    226 }
    227 
    228 } // namespace pacpus
     240    connect (this, SIGNAL(displayLon(double)) , lonVal , SLOT(display(double)));*/
     241}
     242
     243
     244/************************************************************************
     245 * Configuration of the component, called by the ComponentManager after
     246 * the construction of the object
     247 ************************************************************************/
     248ComponentBase::COMPONENT_CONFIGURATION DbtPlyGgaManager::configureComponent(XmlComponentConfig config)
     249{
     250  DbtPlyFileManager::configureComponent(config);
     251
     252  return ComponentBase::CONFIGURED_OK;
     253}
     254

  • trunk/StdDbtPlayerComponents/DbtPlyGgaManager.h

    r82 r103  
    1919#include <qlayout.h>
    2020
     21#include <iostream>
     22
    2123#include "Pacpus/DbitePlayer/DbtPlyFileManager.h"
    22 #include <Pacpus/structure/structure_gps.h>
     24//#include <Pacpus/structure/structure_gps.h>
     25#include "../Gps/structure_gps.h"
    2326#include "Pacpus/kernel/ComponentManager.h"//moved from .cpp
     27
     28#include <Pacpus/kernel/InputOutputInterface.h>
    2429//#include "DbitePlayer/SensorTcpServer.h" //added
    25 
    26 #include "Pacpus/PacpusTools/ShMem.h"
     30//#include "Pacpus/PacpusTools/ShMem.h"
    2731
    2832#include "DbtPlyGpsConfig.h"
    … …  
    3842    DbtPlyGgaManager(QString name);
    3943    ~DbtPlyGgaManager();
     44
     45    virtual void startActivity();
     46    virtual void stopActivity();
    4047    virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config);
    4148
    42 Q_SIGNALS:
    43     void displayLat(double);
    44     void displayLon(double);
     49    virtual void addInputs();
     50    virtual void addOutputs();
    4551
    4652protected:
    4753    void processData(road_time_t t, road_timerange_t tr , void *buf);
    48     virtual void startActivity();
    49     virtual void stopActivity();
    5054    void displayUI();
    5155
    5256private:
    53     trame_gga_dbl* val;
    54     QWidget *w;
     57    // Local copy of GGA data with timestamp
     58    TimestampedGgaFrame mGga;
     59
     60    // Declaration of outputs
     61    OutputInterface<TimestampedGgaFrame, DbtPlyGgaManager>* outGga;
     62
     63    // Variables for GUI : TODO
     64    /*QWidget *w;
    5565    QLabel* lat;
    5666    QLCDNumber *latVal;
    5767    QLabel* lon;
    5868    QLCDNumber *lonVal;
    59     QGroupBox *prop;
     69    QGroupBox *prop;*/
    6070
    6171};
Note: See TracChangeset for help on using the changeset viewer.