source: pacpussensors/trunk/Gps/UbloxComponent.cpp@ 69

Last change on this file since 69 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: 6.9 KB
RevLine 
[59]1#include "UbloxComponent.h"
2
3
4
5// Construct the factory
6static ComponentFactory<ubloxComponent>* factory = new ComponentFactory<ubloxComponent>("UbloxComponent");
7
8
9
10ubloxComponent::ubloxComponent(QString name) : semaphore(INT_MAX), ComponentBase(name)
11{
12
13 //create the message handler
14 pHandleStream = new SerialCOM_Handle_Stream(false);
15
16 //create protocol handler to use
17 pProtocolUBX = new Ublox::SerialCOM_Protocol_UBX();
18 pProtocolNMEA = new NMEA::SerialCOM_Protocol_NMEA();
19
20 //create message handler for attended messages and add it to the relevant protocol
21 //ATTENTION don't forget to free the memory by calling pProtocolUBX->clear()
22 pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_RXM_RAW());
23 pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_RXM_SFRB());
24 pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_POSLLH());
25 pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_VELNED());
26 pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_POSUTM());
27 pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_CLOCK());
28 pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_SOL());
29 pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_SBAS());
30 pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_SVINFO());
31
32 pHandleStream->addProtocol(pProtocolUBX);
33 pHandleStream->addProtocol(pProtocolNMEA);
34
35 currentDataFrame = "";
36 currentRoadtime = 0;
37 currentTimerange = 0;
38
39 // put the sempahore count to 0
40 semaphore.acquire( INT_MAX );
41}
42
43ubloxComponent::~ubloxComponent()
44{
45 pProtocolUBX->clear();
46 delete pProtocolUBX;
47 delete pHandleStream;
48}
49
50 /*!< to start the processing thread */
51void ubloxComponent::startActivity()
52{
53 qDebug() << "start";
54 THREAD_ALIVE = TRUE;
55
56#if WIN32
57 serialPort = new Win32SerialPort(portName.toLatin1());
58 // Asynchrone
59 //serialPort->setMode(FILE_FLAG_OVERLAPPED);
60 // Synchrone
61 serialPort->setMode(0);
62#else
63 serialPort = new PosixSerialPort(portName.toLatin1());
64#endif
65
66 if (!serialPort->openPort(portName.toLatin1()))
67 {
68 qDebug() << "Failed to open the port " << portName;
69 qDebug() << "The GPS Component " << componentName << " didn't start";
70 return;
71 }
72
73 serialPort->THREAD_ALIVE = TRUE;
74 if (!QApplication::connect(serialPort,SIGNAL(newDataAvailable(int)),this, SLOT(unlockProcessing(int))))
75 qWarning("Failed to connect SIGNAL(newDataAvailable(int)) with SLOT(unlockProcessing(int)\n" );
76 start();
77 qDebug() << "The Component " << componentName << " is started";
78}
79
80 /*!< to stop the processing thread */
81void ubloxComponent::stopActivity()
82{
83 unlockProcessing(1);
84 THREAD_ALIVE = FALSE;
85 serialPort->THREAD_ALIVE = FALSE;
86 if (!serialPort->wait(2000))
87 {
88 serialPort->terminate();
89 qDebug("The Win32SerialPort thread blocks anormally, it has been killed !!");
90 }
91 if ( !serialPort->closePort() )
92 qDebug("Failed to close the port");
93 else
94 qDebug("The port is closed");
95 delete serialPort;
96}
97
98
99ComponentBase::COMPONENT_CONFIGURATION ubloxComponent::configureComponent(XmlComponentConfig config)
100{
101 portName = param.getProperty("port").toLatin1();
102 recording = (param.getProperty("recording") == "true" ? true : false);
103
104 return ComponentBase::CONFIGURED_OK;
105}
106
107
108
109/*!< the main loop of the thread */
110void ubloxComponent::run()
111{
112 bool NMEApresent = false;
113 bool unknownUbloxpresent = false;
114
115 qDebug() << "run";
116
117 FRAME *currentFrame = NULL;
118 unsigned char *buffer = NULL;
119
120
121 // parity:0-4=no,odd,even,mark,space
122 // byteSize:number of bits/byte, 4-8
123 // baudRate:port speed (ex:38400)
124 // stopBits:0,1,2 = 1, 1.5, 2
125 serialPort->configurePort(param.getProperty("baudrate").toLong(),
126 param.getProperty("bytesize").toUInt(),
127 param.getProperty("parity").at(0).toLatin1(),
128 param.getProperty("stopbits").toUInt());
129 serialPort->start();
130
131
132 while (THREAD_ALIVE)
133 {
134 // try to get access to decode the frame
135 semaphore.acquire();
136 if (!THREAD_ALIVE)
137 continue;
138
139 if (serialPort->numberOfFrames())
140 {
141 currentFrame = serialPort->firstFrame(); // get the frame ..
142 if (currentFrame == NULL)
143 {
144 qDebug() << "UbloxComponent::run() : bad currentFrame in component " << componentName;
145 continue;
146 }
147
148 buffer = new unsigned char[currentFrame->length + 1];
149 if (buffer == NULL)
150 {
151 qDebug() << "GpsComponent::run() : Failed to alloc memory in component " << componentName;
152 continue;
153 }
154
155 memcpy(buffer,currentFrame->data,currentFrame->length);
156 int bufferLength = (int)currentFrame->length;
157 currentRoadtime = currentFrame->t;
158 currentTimerange = currentFrame->tr;
159
160 serialPort->removeFirstFrame();
161
162 int iStartBuffer = 0; //new data, so start to read the buffer at 0
163 int nbBytesRead = 0;
164
165 //while no error and we don't reach the end of the buffer
166 do{
167 if (pHandleStream->addData(buffer,bufferLength,iStartBuffer,&nbBytesRead, (long double)currentRoadtime))
168 {
169 // a new complete message
170 SerialCOM_Msg *pCurMsg = pHandleStream->getCurMsgPointer();
171
172 if (pCurMsg!=NULL)
173 {
174 if ( (recording) && (pCurMsg->recording) )
175 {
176 if (!pCurMsg->DByteFileHeader)
177 pCurMsg->DByteFileHeader = inithdFile((char *)(componentName + "_" + QString(pCurMsg->getName().c_str()) +".dbt").toLatin1().data(),
178 pCurMsg->getDbtType(),
179 pCurMsg->getSizeOfStruct());//sizeof(*pCurMsg));
180 if ( pCurMsg->writeDByte((road_time_t)pCurMsg->getMsgTime(),currentTimerange) == 0)
181 qWarning("Failed to record this data ...\n");
182 }
183 }
184 else
185 {
186 if(pHandleStream->getCurProtocolID()==1) //display one time the NMEA message presence warning
187 if (!NMEApresent)
188 {
189 NMEApresent = true;
190 qDebug("NMEA message(s) present but not recorded \n");
191 };
192 if(pHandleStream->getCurProtocolID()==2) //display one time the Ublox unknown message presence warning
193 if (!unknownUbloxpresent)
194 {
195 unknownUbloxpresent = true;
196 qDebug("unknown Ublox message(s) present so not recorded \n");
197 };
198 if((nbBytesRead==0) && (bufferLength!=0))
199 {
200 //qDebug("problem in the SerialCOM Librairy, Msg found but no pointer to this message");
201 qDebug("problem in the SerialCOM Librairy \n");
202 qDebug(pHandleStream->getInformationText().c_str());
203 }
204 }
205
206 }// END if (pHandleStream->addData(buffer,currentFrame->length,start,&nbRead))
207
208 //cumulate the number of bytes read
209 iStartBuffer = iStartBuffer + nbBytesRead;
210
211 //while no error and we don't reach the end of the buffer
212 }while( (nbBytesRead>0) && (iStartBuffer<bufferLength) );
213 }// END if (serialPort->numberOfFrames())
214
215 delete[] buffer;
216 buffer = NULL;
217 } // END while(THREAD_ALIVE)
218
219 qDebug() << "The thread of " << componentName << " GPS component is stopped";
220}
221
222void ubloxComponent::unlockProcessing(int v)
223{
224 // new frame available
225 semaphore.release( v );
226}
Note: See TracBrowser for help on using the repository browser.