source: pacpussensors/trunk/StdDbtPlayerComponents/DbtPlyGgaManager.cpp

Last change on this file was 103, checked in by xuphilip, 9 years ago

I/O update

File size: 7.2 KB
Line 
1/*********************************************************************
2// created: 2007/04/12 - 16:30
3
4//
5// author: Elie Al Alam & Gerald Dherbomez
6//
7// version: $Id: DbtPlyGgaManager.cpp 1203 2012-08-02 11:58:15Z morasjul $
8//
9// purpose: Dbite Player GGA Manager implementation
10*********************************************************************/
11
12#include "DbtPlyGgaManager.h"
13
14#include <cmath>
15#include <qapplication.h>
16
17using namespace pacpus;
18using namespace std;
19
20/************************************************************************
21 * Construction de la fabrique de composant DbtPlyGgaManager
22 ************************************************************************/
23static ComponentFactory<DbtPlyGgaManager> sFactory("DbtPlyGgaManager");
24
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/*
113double atanh(double z)
114{
115 return (0.5 * log((z+1)/(1-z)));
116}
117
118double abs_me(double x)
119{
120 if(x<0) x=-x;
121 return x;
122}
123
124//////////////////////////////////////////////////////////////////////////
125//transformation des longitudes tatitudes en Lambert93
126//////////////////////////////////////////////////////////////////////////
127void lonlattolam(double lon, double lat, double & lam93x, double & lam93y)
128{
129 double GRS_a = 6378137;
130 double GRS_f = 1/298.257222101;
131
132 double GRS_b = GRS_a*(1-GRS_f);
133 double GRS_bb= GRS_b*GRS_b;
134 double GRS_aa= 40680631590769.0;
135 double GRS_e = sqrt((GRS_aa - GRS_bb) / (GRS_aa));
136
137 double n = 0.725607765053267;
138 double C = 11754255.4261;
139 double XS = 700000;
140 double YS = 12655612.0499;
141
142 double latiso;
143 latiso = atanh(sin(lat)) - GRS_e*atanh(GRS_e*sin(lat));
144 double gamma;
145 gamma = (lon - 0.0523598775598299)*n;
146 double R;
147 R = C * exp(-n*latiso);
148
149 lam93x = R *sin(gamma)+XS;
150 lam93y = -R *cos(gamma)+YS;
151}
152
153//////////////////////////////////////////////////////////////////////////
154//Transformation des coordonnees de Lambert93 en Longitude lattitude
155//////////////////////////////////////////////////////////////////////////
156void lamtolonlat(double lamx, double lamy, double & lon, double & lat)
157{
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;
200 }
201
202 lat = phiNew;
203}*/
204
205void DbtPlyGgaManager::displayUI()
206{
207 // TODO
208 LOG_WARN("GUI not implemented");
209
210 // afficher lat lon
211 // voir dans DbtPlyUserInterface§.cpp
212 /*w = new QWidget();
213 w->setWindowTitle(name());
214 w->show();
215 w->setFixedSize (170,80);
216
217 prop = new QGroupBox ("GGA Properties",w);
218 prop->show();
219 prop->setFixedSize(170,78);
220
221 lat = new QLabel("Latitude ", prop);
222 lat->move( 5, 15);
223 lat->show();
224 lat->setFixedSize (80,20);
225 latVal = new QLCDNumber (prop);
226 latVal->move( 5, 35);
227 latVal->setFixedSize(50,20);
228 latVal->show();
229 connect(this, SIGNAL(displayLat(double)) , latVal , SLOT(display(double)));
230
231 lon = new QLabel("Longitude ", prop);
232 lon->move( 90, 15);
233 lon->show();
234 lon->setFixedSize (80,27);
235
236 lonVal = new QLCDNumber (prop);
237 lonVal->move( 90, 35);
238 lonVal->setFixedSize(50,20);
239 lonVal->show();
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
Note: See TracBrowser for help on using the repository browser.