source: pacpussensors/trunk/Gps/UbloxComponent.cpp

#include "UbloxComponent.h"



// Construct the factory
static ComponentFactory<ubloxComponent>* factory = new ComponentFactory<ubloxComponent>("UbloxComponent"); 



ubloxComponent::ubloxComponent(QString name) : semaphore(INT_MAX), ComponentBase(name)
{
  
        //create the message handler
        pHandleStream = new SerialCOM_Handle_Stream(false);

        //create protocol handler to use
        pProtocolUBX = new Ublox::SerialCOM_Protocol_UBX();
        pProtocolNMEA = new NMEA::SerialCOM_Protocol_NMEA();

        //create message handler for attended messages and add it to the relevant protocol
        //ATTENTION don't forget to free the memory by calling pProtocolUBX->clear()
        pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_RXM_RAW());
        pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_RXM_SFRB());
        pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_POSLLH());
        pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_VELNED());
        pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_POSUTM());
        pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_CLOCK());
        pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_SOL());
        pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_SBAS());
        pProtocolUBX->addMsg(new Ublox::SerialCOM_Msg_NAV_SVINFO());

        pHandleStream->addProtocol(pProtocolUBX);
        pHandleStream->addProtocol(pProtocolNMEA);

  currentDataFrame = ""; 
  currentRoadtime = 0; 
  currentTimerange = 0; 
  
  // put the sempahore count to 0
  semaphore.acquire( INT_MAX );
}

ubloxComponent::~ubloxComponent()
{
        pProtocolUBX->clear();
        delete pProtocolUBX;
        delete pHandleStream;   
}

 /*!< to start the processing thread */
void ubloxComponent::startActivity()
{
        qDebug() << "start";
  THREAD_ALIVE = TRUE;   

#if WIN32
  serialPort = new Win32SerialPort(portName.toLatin1());  
  // Asynchrone
  //serialPort->setMode(FILE_FLAG_OVERLAPPED); 
  // Synchrone
  serialPort->setMode(0); 
#else 
  serialPort = new PosixSerialPort(portName.toLatin1());  
#endif

  if (!serialPort->openPort(portName.toLatin1()))
  {
    qDebug() << "Failed to open the port " << portName;
    qDebug() << "The GPS Component " << componentName << " didn't start";
    return; 
  }
  
  serialPort->THREAD_ALIVE = TRUE; 
  if (!QApplication::connect(serialPort,SIGNAL(newDataAvailable(int)),this, SLOT(unlockProcessing(int))))
    qWarning("Failed to connect SIGNAL(newDataAvailable(int)) with SLOT(unlockProcessing(int)\n" );   
  start(); 
  qDebug() << "The Component " << componentName << " is started";
}

 /*!< to stop the processing thread */ 
void ubloxComponent::stopActivity()
{
        unlockProcessing(1);
        THREAD_ALIVE = FALSE; 
        serialPort->THREAD_ALIVE = FALSE; 
        if (!serialPort->wait(2000))
        {
                serialPort->terminate(); 
                qDebug("The Win32SerialPort thread blocks anormally, it has been killed !!");
        }
        if ( !serialPort->closePort() ) 
                qDebug("Failed to close the port"); 
        else
                qDebug("The port is closed"); 
        delete serialPort;
}


ComponentBase::COMPONENT_CONFIGURATION ubloxComponent::configureComponent(XmlComponentConfig config)
{
        portName = param.getProperty("port").toLatin1(); 
        recording = (param.getProperty("recording") == "true" ? true : false);

        return ComponentBase::CONFIGURED_OK;
}



/*!< the main loop of the thread */ 
void ubloxComponent::run()
{
        bool NMEApresent = false;
        bool unknownUbloxpresent = false;

        qDebug() << "run";

        FRAME *currentFrame = NULL;  
        unsigned char *buffer = NULL; 


        // parity:0-4=no,odd,even,mark,space
        // byteSize:number of bits/byte, 4-8
        // baudRate:port speed (ex:38400) 
        // stopBits:0,1,2 = 1, 1.5, 2 
        serialPort->configurePort(param.getProperty("baudrate").toLong(), 
                                                          param.getProperty("bytesize").toUInt(),
                                                          param.getProperty("parity").at(0).toLatin1(),
                                                          param.getProperty("stopbits").toUInt()); 
        serialPort->start(); 
    

  while (THREAD_ALIVE)
  {
    // try to get access to decode the frame
    semaphore.acquire(); 
    if (!THREAD_ALIVE)
      continue; 
    
    if (serialPort->numberOfFrames())
    {
                currentFrame = serialPort->firstFrame();                        // get the frame ..
                if (currentFrame == NULL)
                {
                        qDebug() << "UbloxComponent::run() : bad currentFrame in component " << componentName; 
                        continue; 
                }

                buffer = new unsigned char[currentFrame->length + 1]; 
                if (buffer == NULL)
                {
                        qDebug() << "GpsComponent::run() : Failed to alloc memory in component " << componentName; 
                        continue; 
                }

                memcpy(buffer,currentFrame->data,currentFrame->length); 
                int bufferLength = (int)currentFrame->length;
                currentRoadtime = currentFrame->t;
                currentTimerange = currentFrame->tr;

                serialPort->removeFirstFrame();      

                int iStartBuffer = 0; //new data, so start to read the buffer at 0
                int nbBytesRead = 0;

                //while no error and we don't reach the end of the buffer
                do{
                        if (pHandleStream->addData(buffer,bufferLength,iStartBuffer,&nbBytesRead, (long double)currentRoadtime))
                        {
                        // a new complete message 
                                SerialCOM_Msg *pCurMsg = pHandleStream->getCurMsgPointer();

                                if (pCurMsg!=NULL)
                                {
                                        if ( (recording) && (pCurMsg->recording) )
                                        {
                                                if (!pCurMsg->DByteFileHeader)
                                                        pCurMsg->DByteFileHeader = inithdFile((char *)(componentName + "_" + QString(pCurMsg->getName().c_str()) +".dbt").toLatin1().data(),
                                                        pCurMsg->getDbtType(),
                                                        pCurMsg->getSizeOfStruct());//sizeof(*pCurMsg));
                                                if ( pCurMsg->writeDByte((road_time_t)pCurMsg->getMsgTime(),currentTimerange) == 0)
                                                        qWarning("Failed to record this data ...\n"); 
                                        }
                                }
                                else
                                {
                                        if(pHandleStream->getCurProtocolID()==1) //display one time the NMEA message presence warning
                                                if (!NMEApresent)
                                                {
                                                        NMEApresent = true;
                                                        qDebug("NMEA message(s) present but not recorded \n");
                                                };
                                        if(pHandleStream->getCurProtocolID()==2) //display one time the Ublox unknown message presence warning
                                                if (!unknownUbloxpresent)
                                                {
                                                        unknownUbloxpresent = true;
                                                        qDebug("unknown Ublox message(s) present so not recorded \n");
                                                };
                                        if((nbBytesRead==0) && (bufferLength!=0))
                                        {
                                                //qDebug("problem in the SerialCOM Librairy, Msg found but no pointer to this message");
                                                qDebug("problem in the SerialCOM Librairy \n");
                                                qDebug(pHandleStream->getInformationText().c_str());
                                        }
                                }

                        }// END if (pHandleStream->addData(buffer,currentFrame->length,start,&nbRead))

                        //cumulate the number of bytes read
                        iStartBuffer = iStartBuffer + nbBytesRead;

                //while no error and we don't reach the end of the buffer
                }while(  (nbBytesRead>0) && (iStartBuffer<bufferLength) ); 
        }// END if (serialPort->numberOfFrames())

        delete[] buffer; 
        buffer = NULL; 
  } // END while(THREAD_ALIVE)

  qDebug() << "The thread of " << componentName << " GPS component is stopped"; 
}

void ubloxComponent::unlockProcessing(int v)
{
  // new frame available
  semaphore.release( v ); 
}