GPSSensor.cpp

00001 
00002 
00003 #include "GPSSensor.h"
00004 
00005 #define __DEBUG__ 2
00006 #ifndef __MODULE__
00007 #define __MODULE__ "GPSSensor.cpp"
00008 #endif
00009 #include "MyDebug.h"
00010 
00011 #define GPS_THREAD_STACK_SIZE   768
00012 
00013 bool gpsNewlineDetected;
00014 
00015 void rxFullCallback(MODSERIAL_IRQ_INFO *q){
00016     DBG("OVF");
00017     MODSERIAL *serial = q->serial;
00018     serial->rxBufferFlush();
00019 }
00020 
00021 void rxCallback(MODSERIAL_IRQ_INFO *q) {
00022     MODSERIAL *serial = q->serial;
00023     if ( serial->rxGetLastChar() == '\n') {
00024         gpsNewlineDetected = true;
00025     }
00026 }
00027 
00028 // Well there is an issue in the RX Serial data, sometime multiples frames collides between themselves... 
00029 // To debug when we gonna have GSP Signal 
00030 
00031 GPSSensor::GPSSensor(PinName tx, PinName rx, uint32_t trackingTime, uint32_t idle):
00032     MySensor(SENSOR_NAME_GPS, SENSOR_TYPE_GPS, idle, GPS_THREAD_STACK_SIZE),
00033     gps(tx,rx) {
00034     fixed = false;
00035     trackTime = trackingTime;
00036     lastImpact = 0;
00037     gpsNewlineDetected = false;
00038     gps.baud(9600);
00039     gps.rxBufferFlush();
00040     gps.txBufferFlush();
00041     WARN("Increase the baudrate to 57600");
00042     gps.printf("$PUBX,41,1,0007,0003,57600,0*2B\r\n");
00043     Thread::wait(1000);
00044     gps.baud(57600);
00045     WARN("Disabling all unused NMEA sentences");
00046     gps.printf("$PUBX,40,GLL,0,0,0,0,0,0*5C\r\n");
00047     gps.printf("$PUBX,40,ZDA,0,0,0,0,0,0*44\r\n");
00048     gps.printf("$PUBX,40,VTG,0,0,0,0,0,0*5E\r\n");
00049     gps.printf("$PUBX,40,GSV,0,0,0,0,0,0*59\r\n");
00050     gps.printf("$PUBX,40,GSA,0,0,0,0,0,0*4E\r\n");
00051     //gps.printf("$PUBX,40,RMC,0,0,0,0,0,0*47\r\n");
00052     Thread::wait(500);
00053     gps.rxBufferFlush();
00054     gps.txBufferFlush();
00055     WARN("Setup Callbacks");
00056     gps.attach(&rxCallback, MODSERIAL::RxIrq);
00057     gps.attach(&rxFullCallback, MODSERIAL::RxOvIrq);
00058     InitResults(SENSOR_RESSZ_DEFAULT);
00059     WARN("Finish Init");
00060 }
00061 
00062 // Could we have a way to put the InitResultStatic/StoreLastImpact into the base class?
00063 
00064 void GPSSensor::InitResultsStatic(void) {
00065     DBG("InitResultStatic have been defined");
00066     results.start = (uint8_t*)store;
00067     results.current = (uint8_t*)store;
00068     results.max = sizeof(store); //IMU_STORE_SIZE*sizeof(uint16_t);
00069 }
00070 
00071 void GPSSensor::Loop(void) {
00072     // We read all the available data as fast as possible 
00073     // but giving opportunity to be preampted (wait 100ms)
00074     // Doing this reading should avoid serial internal buffer to ovf
00075      Sample();
00076      StoreLastImpact();
00077      Thread::wait(idleTime);
00078 }
00079 
00080 void GPSSensor::StoreLastImpact(void) {
00081     if(fixed && NeedImpact()) {
00082         if((results.max - results.num)>=sizeof(gpsImpact)){
00083             DBG("===========> Storing New impact <===========");
00084             if(resultsMutex.trylock()) {
00085                 memcpy((char*)results.current,(const char*)&impact,sizeof(gpsImpact));
00086                 results.current += sizeof(gpsImpact);
00087                 results.num += sizeof(gpsImpact);
00088                 DBG("%p %d %d",results.current,results.num,sizeof(gpsImpact));
00089                 resultsMutex.unlock();
00090             }
00091         } else
00092             ERR("No Space to Store results... Data will be overwrited at next Loop");
00093     } else {
00094         DBG("Nope, (%s)", (!fixed)?"not fixed":"no impact needed");
00095     }
00096 }
00097 
00098 bool GPSSensor::NeedImpact(void) {
00099     time_t now = time(NULL);
00100     DBG("%ld-%ld = %d > %d ",now,lastImpact,now-lastImpact,trackTime);
00101     if((now-lastImpact)>trackTime){
00102         DBG("NEED IMPACT");
00103         lastImpact = now;
00104         return true;
00105     }
00106     return false;
00107 }
00108 
00109 int GPSSensor::GetLine(void) {
00110     int len = 0;
00111     // Consume the chars until we found a '$'
00112     while (gps.getc() != '$');
00113     recvBuff[0] = '$';
00114     // Fill the GPS_RECV_BUFF 
00115     for (int i=1; i<(GPS_RECV_BUFF-2); i++) {
00116         recvBuff[i] = gps.getc();
00117         len++;
00118         if (recvBuff[i] == '\n') {
00119             recvBuff[i+1] = '\0';
00120             return len;
00121         }        
00122     }
00123     error("Overflow in getline");
00124     return GPS_RECV_BUFF-2;
00125 }
00126 
00127 void GPSSensor::Sample(void) {
00128     int len = 0;
00129     bool newData = false;
00130     if(gpsNewlineDetected) {
00131         while(1) {
00132             int nChars = gps.rxBufferGetCount();
00133             if(nChars == 0) 
00134                 break;
00135             //else
00136             //    DBG("To read --> %d",nChars);
00137             // We could consider passing the data directly to parser whithout buffering it
00138             // but: That's annoying cause if so we could not printf them smoothly on console
00139             len = GetLine();
00140             
00141             //DBG("GotLine(%d) %s",len,recvBuff);
00142             // DBG_MEMDUMP("GPSData",(const char*)recvBuff,len);
00143             
00144             for(unsigned int i = 0; i < len; i++) {
00145                 // Insert all data to gpsParser
00146                 if(gpsParser.encode(recvBuff[i])) {
00147                     newData = true;
00148                 }
00149             }
00150             
00151             // Only take a sample when all the data for each epoch have been received
00152             if(newData && gpsParser.rmc_ready() && gpsParser.gga_ready()){
00153                 long lat = 0, lon = 0, hdop = 0;
00154                 unsigned long date = 0, time = 0;
00155                 gpsParser.get_position(&lat,&lon);
00156                 gpsParser.get_datetime(&date,&time);
00157                 hdop = gpsParser.hdop();
00158                 if((lat != TinyGPS::GPS_INVALID_ANGLE) && (lon != TinyGPS::GPS_INVALID_ANGLE)){
00159                     fixed = true;
00160                     impact.date = date;
00161                     impact.time = time/100;
00162                     impact.lat = lat;
00163                     impact.lon = lon;
00164                     if(hdop>0xffff) {
00165                         impact.hdop = -1;
00166                     } else {
00167                         impact.hdop = hdop;
00168                     }
00169                     WARN("######[%ld-%ld][%09ld|%09ld|%04d]######",impact.date,impact.time,impact.lat,impact.lon,impact.hdop);
00170                 } else {
00171                     fixed = false;
00172                     DBG("All data have been received but lat/lon does not seems to be valid",date,time,lat,lon,hdop);
00173                 }
00174                 gpsParser.reset_ready();
00175             } else {
00176                 fixed = false;
00177                 DBG("All frames did not came yet RMC(%c)GGA(%c)newData(%c)",gpsParser.rmc_ready()?'x':'o',gpsParser.gga_ready()?'x':'o',newData?'x':'o');
00178             }
00179         }
00180     }
00181 }
00182 
00183 // OLD SAMPLING 
00184 /*
00185 void GPSSensor::Sample(void) {
00186     int lineParsed = 0;
00187     DBG("Read GPS");
00188     if(gpsNewlineDetected) {
00189         gpsNewlineDetected = false;
00190         while(1) {
00191             int nChars = gps.rxBufferGetCount();
00192             if(nChars == 0) {
00193                 DBG("No data to read anymore on gps line");
00194                 break;
00195             } else {
00196                 DBG("--> %d",nChars);
00197             }
00198                 
00199             DBG("Read Line GPS");
00200             GetLine();
00201             DBG("GotLine, %s",recvBuff);
00202             DBG_MEMDUMP("GPSData",(const char*)recvBuff,strlen((const char*)recvBuff));
00203             
00204             // Stuffs used in GPGGA
00205             
00206             // The only NMEA sentence we gonna have (all other are disabled)
00207             int nArgs = sscanf((const char*)recvBuff, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c",&hour,&lat,&ns,&lon,&ew,&q,&su,&hdop,&alt,&altU);
00208             if(nArgs == 10) {
00209                 DBG("We got all the arguments");
00210                 DBG("%d [%f/%f/%c/%f/%c/%d/%d/%f/%f/%c]\n",nArgs,hour,lat,ns,lon,ew,q,su,hdop,alt,altU);
00211                 lineParsed = GGA;
00212             }
00213             
00214             // if lineParsed is different from zero meens that's we got new data
00215             if(lineParsed) {
00216                 DBG("Impact Result %d", su);
00217                 // When 0 satelites usualy this we can't lock
00218                 if(su == 0)
00219                     q = 0;
00220                 
00221                 if(q!=0){
00222                     DBG("Locked (q = %d)",q);
00223                     fixed = true;
00224                     impact.utc = (uint32_t) hour;
00225                     impact.lat = DegToDec(lat,ns);
00226                     impact.lon = DegToDec(lon,ew);
00227                     impact.alt = alt;
00228                     impact.su = su;
00229                     impact.hdop = hdop;
00230                     DBG("Impact Results %d, %f, %f, %f, %d, %d", impact.utc, impact.lat, impact.lon, impact.alt, impact.su, impact.hdop);
00231                 }
00232             }
00233         }
00234     }
00235 }
00236 */
00237