source: pacpussensors/trunk/Gps/SeptentrioComponent.cpp@ 123

Last change on this file since 123 was 59, checked in by DHERBOMEZ Gérald, 10 years ago

Integration of new modules:

  • GPS NMEA0183 decoder
  • Span CPT Decoder

Update of:

File size: 18.2 KB
RevLine 
[59]1/*********************************************************************
2// created: 2008/07/18 - 15:33
3// filename: SeptentrioComponent.cpp
4//
5// author: Gerald Dherbomez & Clement Fouque
6// Copyright Heudiasyc UMR UTC/CNRS 6599
7//
8// version: $Id: $
9//
10// purpose:
11//
12*********************************************************************/
13
14
15#include <iostream>
16#include <string>
17
18#include <QTcpSocket>
19#include <QApplication>
20
21#include "SeptentrioComponent.h"
22#include "SeptentrioSocket.h"
23#include "kernel/ComponentFactory.h"
24
25// Construct the factory
26static ComponentFactory<SeptentrioComponent>* factory = new ComponentFactory<SeptentrioComponent>("SeptentrioComponent");
27
28
29
30
31//////////////////////////////////////////////////////////////////////////
32// Constructor
33/////////////////////////////////////////////////////////////////////////
34SeptentrioComponent::SeptentrioComponent(QString name) : ComponentBase(name)
35{
36 socketIf_ = new SeptentrioSocket();
37 recording = false;
38 computeGPSTk = false ;
39 // default values
40 host_ = "127.0.0.1";
41 port_ = 80;
42 frame_ = "" ;
43
44 ui_ = 0 ;
45
46 currentRoadtime = 0 ;
47 currentTimerange = 0 ;
48
49 // Create message handler:
50 pHandleStream = new SerialCOM_Handle_Stream( false );
51
52 // Define protocol to be handled
53 pProtocolSBF = new Plrx::SerialCOM_Protocol_SBF();
54
55 // Add message to decode to protocol
56 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_MeasEpoch() );
57 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_ShortMeasEpoch() );
58 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GenMeasEpoch() );
59 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_EndOfMeas() );
60
61 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GPSNav() );
62 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GPSAlm() );
63 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GPSIon() );
64 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GPSUtc() );
65 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GPSRaw() );
66 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_CNAVRaw() );
67
68 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOMT00() );
69 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOPRNMask() );
70 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOFastCorr() );
71 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOIntegrity() );
72 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOFastCorrDegr() );
73 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEONav() );
74 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEODegrFactors() );
75 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEONetworkTime() );
76 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOAlm() );
77 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOIGPMask() );
78 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOLongTermCorr() );
79 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOIonoDelay() );
80 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOServiceLevel() );
81 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOClockEphCovMatrix() );
82 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEORaw() );
83
84 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_PVTCartesian() );
85 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_PVTGeodetic() );
86 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_PosCovCartesian() );
87 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_PosCovGeodetic() );
88 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_VelCovCartesian() );
89 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_VelCovGeodetic() );
90 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_DOP() );
91 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_PVTResiduals() );
92 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_RAIMStatistics() );
93 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_GEOCorrections() );
94 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_BaseLine() );
95 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_EndOfPVT() );
96
97 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_ReceiverTime() );
98 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_xPPSOffset() );
99 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_ExtEvent() );
100
101 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_TrackingStatus() );
102 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_ReceiverStatus() );
103 pProtocolSBF->addMsg( new Plrx::SerialCOM_Msg_ReceiverSetup() );
104
105 // Add protocol to handler:
106 pHandleStream->addProtocol( pProtocolSBF );
107
108 /*
109 recPos = gpstk::Position( 49.3994929 , 2.7988750 , 87.071 , gpstk::Position::Geodetic ) ;
110 curPos = gpstk::Position( 49.3994929 , 2.7988750 , 87.071 , gpstk::Position::Geodetic ) ;
111 curVel = Vector<double>(3) ;
112 curTime = Vector<double>(2) ;
113
114 pSolver = new polarxGPSTKsolver() ;
115 */
116 prevRoadTime = 0 ;
117
118 // Creating Shared Memory
119 pShMem = new QSharedMemory( "polarx_shmem" , this );
120 if ( pShMem->create(16384) )
121 qDebug() << "Shared Memory Segment created" ;
122 else
123 {
124 qDebug() << pShMem->errorString() ;
125 if ( pShMem->error() == 4 )
126 {
127 qDebug() << "Trying to attach ...." ;
128 if ( pShMem->attach() )
129 qDebug() << "done" ;
130 }
131 }
132
133 // Initialize the shared memory
134 pShMem->lock();
135 memset(pShMem->data(), 0, pShMem->size());
136 pShMem->unlock();
137
138 // Creating System Sempahore
139 pShmSem = new QSystemSemaphore( "polarx_sem" , 0 , QSystemSemaphore::Create );
140
141}
142
143
144
145
146
147//////////////////////////////////////////////////////////////////////////
148// Destructor
149//////////////////////////////////////////////////////////////////////////
150SeptentrioComponent::~SeptentrioComponent()
151{
152 delete socketIf_;
153
154 // Delete StreamHandler to free memory
155 pProtocolSBF->clear() ;
156 delete pProtocolSBF ;
157 delete pHandleStream ;
158 delete ui_ ;
159// delete pShMem ;
160
161}
162
163
164
165
166
167
168//////////////////////////////////////////////////////////////////////////
169// Called by the ComponentManager to start the component
170//////////////////////////////////////////////////////////////////////////
171void SeptentrioComponent::startActivity()
172{
173 socketIf_->connectToServer(host_, port_);
174 QString tosend("sso curr pvtgeo+status");
175 if ( !frame_.isEmpty() ) tosend.append("+").append(frame_) ;
176 socketIf_->sendToServer( tosend );
177 if (ui_)
178 ui_->setSocket( socketIf_ ) ;
179 THREAD_ALIVE = true;
180 start();
181}
182
183
184
185//////////////////////////////////////////////////////////////////////////
186// Called by the ComponentManager to stop the component
187//////////////////////////////////////////////////////////////////////////
188void SeptentrioComponent::stopActivity()
189{
190 THREAD_ALIVE = false;
191 socketIf_->closeSocket();
192 if (!wait(1500))
193 {
194 terminate();
195 qDebug() << "The SeptentrioComponent thread(" << componentName << ")blocks anormally, it has been killed !!";
196 }
197}
198
199
200
201//////////////////////////////////////////////////////////////////////////
202// Called by the ComponentManager to pass the XML parameters to the
203// component
204//////////////////////////////////////////////////////////////////////////
205ComponentBase::COMPONENT_CONFIGURATION SeptentrioComponent::configureComponent(XmlComponentConfig config)
206{
207 if (param.getProperty("recording") != QString::null)
208 recording = ( param.getProperty("recording") == "true" ? true : false );
209 if (param.getProperty("receiverIP") != QString::null)
210 host_ = param.getProperty("receiverIP");
211 if (param.getProperty("receiverPort") != QString::null)
212 port_ = param.getProperty("receiverPort").toInt();
213
214 if ( param.getProperty("sbfFrame") != QString::null )
215 frame_ = param.getProperty("sbfFrame") ;
216 if (param.getProperty("computeGPSTk") != QString::null)
217 computeGPSTk = ( param.getProperty("computeGPSTk") == "true" ? true : false );
218 if (param.getProperty("showUI") == "true")
219 ui_ = new PolarxMainWindow();
220
221
222 return ComponentBase::CONFIGURED_OK;
223}
224
225
226
227
228//////////////////////////////////////////////////////////////////////////
229// Main loop of the thread
230//////////////////////////////////////////////////////////////////////////
231void SeptentrioComponent::run()
232{
233
234 hdfile_t * DBTHeader;
235 // Create DBT File for recording SBF frame
236 if ( recording )
237 {
238 DBTHeader = inithdFile((char *)(componentName + "_SbfFrame.dbt").toLatin1().data() , DBT_SBF_FRAMERAW , sizeof(SbfFrame) );
239 }
240
241 unsigned char *buffer = NULL;
242
243 while (THREAD_ALIVE)
244 {
245 // Get the Frame
246 socketIf_->sem.acquire();
247 SbfFrame frame;
248 frame = socketIf_->sbfFrames.front();
249 socketIf_->sbfFrames.pop();
250
251 if (ui_)
252 ui_->setData( frame.data , frame.size );
253
254 currentRoadtime = frame.time ;
255 currentTimerange = frame.timerange ;
256
257 // Record SbfFrame:
258 if (recording)
259 write_hdfile( DBTHeader , &frame , currentRoadtime , currentTimerange , sizeof(SbfFrame) );
260
261 // Add Buffer to the Handle Stream:
262 int iStartBuffer = 0; //new data, so start to read the buffer at 0
263 int nbBytesRead = 0;
264 int bufferLength = frame.size ;
265 bool flag ;
266 double TOW;
267 int nbMeasReceived = 0 ;
268 do
269 {
270 if ( pHandleStream->addData( frame.data , frame.size , iStartBuffer , &nbBytesRead , (long double)currentRoadtime ) )
271 {
272
273 flag = false ;
274 SerialCOM_Msg *pCurMsg = pHandleStream->getCurMsgPointer();
275 if ( pCurMsg != NULL )
276 {
277
278
279 if ( pCurMsg->getID() == 5913 )
280 {
281 if (ui_)
282 ui_->setReceiverStatus( static_cast<Plrx::SerialCOM_Msg_ReceiverStatus *>(pCurMsg) );
283 }
284 if ( pCurMsg->getID() == 5912 )
285 {
286 if (ui_)
287 ui_->setTrackingStatus( static_cast<Plrx::SerialCOM_Msg_TrackingStatus *>(pCurMsg) );
288 }
289
290 // GPS ECEF Pos:
291 /*
292 if ( pCurMsg->getID() == 5904 )
293 {
294 const double pi = 3.1415926535898;
295 const double r2d = 180. / pi; // M_PI
296 Plrx::SerialCOM_Msg_PVTGeodetic * RecPosData = static_cast<Plrx::SerialCOM_Msg_PVTGeodetic *>(pCurMsg) ;
297 if (ui_)
298 ui_->setPVTGeodetic( RecPosData );
299 try {
300 recPos.setGeodetic( r2d * RecPosData->lat , r2d * RecPosData->lon , RecPosData->h );
301 }
302 catch (gpstk::GeometryException &ge)
303 {
304 // We don't have a valid receiver position
305 recPos.setECEF(-1.e-99, -1.e-99, -1.e-99);
306 }
307 }
308 */
309
310 // GPS Nav frame:
311 if ( pCurMsg->getID() == 5895 )
312 {
313
314 Plrx::SerialCOM_Msg_GPSRaw * NavData = static_cast<Plrx::SerialCOM_Msg_GPSRaw *>(pCurMsg) ;
315 //if ( computeGPSTk) pSolver->addNavigationData( (SbfDataGPSRaw *)NavData );
316 sendToShmem( 5895 , (SbfDataGPSRaw *)NavData, currentRoadtime, currentTimerange );
317 if ( abs(pCurMsg->getMsgTime() - prevRoadTime) < 5000 )
318 {
319 currentRoadtime = prevRoadTime + 5000 ;
320 prevRoadTime = currentRoadtime ;
321 }
322 else
323 prevRoadTime = pCurMsg->getMsgTime() ;
324 }
325
326 // GPS Meas frame:
327 if ( pCurMsg->getID() == 5889 )
328 {
329 Plrx::SerialCOM_Msg_MeasEpoch * MeasData = static_cast<Plrx::SerialCOM_Msg_MeasEpoch *>(pCurMsg) ;
330 sendToShmem( 5889 , (SbfDataMeasEpoch *)MeasData, currentRoadtime, currentTimerange );
331
332 TOW = MeasData->TOW ;
333 nbMeasReceived = MeasData->Nsb ;
334
335// qDebug() << "Raw C1 measurements" ;
336// for ( int k = 0 ; k < MeasData->Nsb ; k++ )
337// {
338// qDebug("%i\t%7.3f" , MeasData->ChannelData[k].SVID , MeasData->ChannelData[k].CACode ) ;
339// }
340
341 //if ( computeGPSTk) pSolver->addObservationData( (SbfDataMeasEpoch *)MeasData );
342 flag = true ;
343 }
344
345 if ( pCurMsg->getID() == 5890 )
346 {
347 Plrx::SerialCOM_Msg_ShortMeasEpoch * MeasData = static_cast<Plrx::SerialCOM_Msg_ShortMeasEpoch *>(pCurMsg) ;
348 TOW = MeasData->TOW ;
349 nbMeasReceived = MeasData->N ;
350 //if ( computeGPSTk) pSolver->addObservationData( MeasData );
351 flag = true ;
352 }
353
354 if ( pCurMsg->getID() == 5944 )
355 {
356 Plrx::SerialCOM_Msg_GenMeasEpoch * MeasData = static_cast<Plrx::SerialCOM_Msg_GenMeasEpoch *>(pCurMsg) ;
357 TOW = MeasData->TOW ;
358 nbMeasReceived = MeasData->N ;
359 //if ( computeGPSTk) pSolver->addObservationData( MeasData );
360 flag = true ;
361 }
362
363 //Compute PolarX position using GPSTK;
364 /*if (computeGPSTk & flag)
365 {
366 if ( !pSolver->computeNavigationSolution() )
367 {
368 curPos = pSolver->getReceiverPosition() ;
369 curVel = pSolver->getReceiverVelocity() ;
370 curTime = pSolver->getReceiverClock();
371 double dx = curPos.X() - recPos.X() ;
372 double dy = curPos.Y() - recPos.Y() ;
373 double dz = curPos.Z() - recPos.Z() ;
374 double d = sqrt( pow(dx,2)+pow(dy,2)+pow(dz,2) );
375
376 qDebug("[ %6.2fs ] : Available meas. : %i (%i)" , TOW , pSolver->NbMeas() , nbMeasReceived ) ;
377 qDebug(" Nb Eph stored : %i" , pSolver->NbEph() ) ;
378 pSolver->EphDump() ;
379 qDebug("*------------------------------------------------------*");
380 qDebug(" GPSTK SOLUTION ");
381 qDebug("X = %7.2f\t Y = %7.2f\t Z = %7.2f" , curPos.X() , curPos.Y() , curPos.Z() );
382 qDebug("Vx = %3.2f\t Vy = %3.2f\t Vz = %3.2f",curVel[0],curVel[1],curVel[2] );
383 qDebug("dt = %3.2f\t ddt = %3.2f\t",curTime[0],curTime[1]);
384 qDebug("dX = %7.2f\t dY = %7.2f\t dZ = %7.2f" , dx , dy , dz );
385 qDebug("|E| = %7.2f" , d );
386 qDebug("*------------------------------------------------------*\n");
387 }
388 else
389 qDebug("[ %6.2fs ] : Unable to compute : %i %i (%i)" , TOW , pSolver->NbEph() , pSolver->NbMeas() , nbMeasReceived ) ;
390 }
391 */
392 // GEO Corrections frame
393 if ( pCurMsg->getID() == 5935 )
394 {
395 qDebug() << "GeoCorr Reiceved" ;
396 }
397
398
399 // DeDBytage des donnees
400 if ( (recording) && (pCurMsg->recording) )
401 {
402 if (!pCurMsg->DByteFileHeader)
403 pCurMsg->DByteFileHeader = inithdFile((char *)(componentName + "_" + QString(pCurMsg->getName().c_str()) +".dbt").toLatin1().data(),
404 pCurMsg->getDbtType(),
405 pCurMsg->getSizeOfStruct());//sizeof(*pCurMsg));
406// if ( pCurMsg->writeDByte( (road_time_t)pCurMsg->getMsgTime() , currentTimerange ) == 0)
407// qWarning("Failed to record this data ...\n");
408 if ( pCurMsg->writeDByte( currentRoadtime , currentTimerange ) == 0)
409 qWarning("Failed to record this data ...\n");
410 }
411 }
412 }
413
414 // Increment number of bytes read
415 iStartBuffer = iStartBuffer + nbBytesRead;
416 }
417 while ( (nbBytesRead>0) && (iStartBuffer<bufferLength) );
418
419 }
420
421}
422
423template <typename T> void SeptentrioComponent::sendToShmem( short id , T * msg, road_time_t t, road_timerange_t tr )
424{
425// // Old 1-message solution
426// pShMem->lock() ;
427// memcpy( (short *)pShMem->data() , &id , sizeof(short) );
428// memcpy( (T *)pShMem->data()+sizeof(short) , msg , sizeof(T) );
429// pShMem->unlock();
430// pShmSem->release();
431
432 QBuffer buffer_in;
433 QDataStream in(&buffer_in);
434
435 QBuffer buffer_out;
436 buffer_out.open(QBuffer::ReadWrite);
437 QDataStream out(&buffer_out);
438
439 QQueue<QByteArray> msgQueue;
440
441 // Lock shared memory
442 pShMem->lock();
443 // Retrieve the current message queue
444 buffer_in.setData((char*)pShMem->constData(), pShMem->size());
445 buffer_in.open(QBuffer::ReadOnly);
446 in >> msgQueue;
447 // Enqueue the new message
448 QByteArray newmsg;
449 newmsg.append( (char*) &id, sizeof(short));
450 newmsg.append( (char*) msg, sizeof(T));
451 newmsg.append( (char*) &t, sizeof(road_time_t));
452 newmsg.append( (char*) &tr, sizeof(road_timerange_t));
453
454 msgQueue.enqueue(newmsg);
455
456 // Try to update the shared memory message queue
457 out << msgQueue;
458 int size = buffer_out.size();
459
460 //qDebug() << "msgQueue is now " << msgQueue.size() << " elmts (" << buffer_out.size() << " bytes).";
461 // New message will be lost if not enough memory
462 if (size <= pShMem->size())
463 {
464 char *to = (char*)pShMem->data();
465 const char *from = buffer_out.data().data();
466 memcpy(to, from, qMin(pShMem->size(), size));
467
468 // Signal the new data
469 pShmSem->release();
470 }
471 else
472 {
473 qDebug("SeptentrioComponent: Message lost.");
474 }
475 // Unlock shared memory
476 pShMem->unlock();
477}
Note: See TracBrowser for help on using the repository browser.