This my big project for PYRN Board
Dependencies: CAN HTTPClient MODSERIAL MyThings Pyrn3GModem Socket TinyGPS MyUSBHost lwip-sys lwip mbed-rtos mbed-src
Diff: GPSSensor.cpp
- Revision:
- 0:efe6085327fd
diff -r 000000000000 -r efe6085327fd GPSSensor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPSSensor.cpp Tue Apr 14 13:30:02 2015 +0000 @@ -0,0 +1,237 @@ + + +#include "GPSSensor.h" + +#define __DEBUG__ 2 +#ifndef __MODULE__ +#define __MODULE__ "GPSSensor.cpp" +#endif +#include "MyDebug.h" + +#define GPS_THREAD_STACK_SIZE 768 + +bool gpsNewlineDetected; + +void rxFullCallback(MODSERIAL_IRQ_INFO *q){ + DBG("OVF"); + MODSERIAL *serial = q->serial; + serial->rxBufferFlush(); +} + +void rxCallback(MODSERIAL_IRQ_INFO *q) { + MODSERIAL *serial = q->serial; + if ( serial->rxGetLastChar() == '\n') { + gpsNewlineDetected = true; + } +} + +// Well there is an issue in the RX Serial data, sometime multiples frames collides between themselves... +// To debug when we gonna have GSP Signal + +GPSSensor::GPSSensor(PinName tx, PinName rx, uint32_t trackingTime, uint32_t idle): + MySensor(SENSOR_NAME_GPS, SENSOR_TYPE_GPS, idle, GPS_THREAD_STACK_SIZE), + gps(tx,rx) { + fixed = false; + trackTime = trackingTime; + lastImpact = 0; + gpsNewlineDetected = false; + gps.baud(9600); + gps.rxBufferFlush(); + gps.txBufferFlush(); + WARN("Increase the baudrate to 57600"); + gps.printf("$PUBX,41,1,0007,0003,57600,0*2B\r\n"); + Thread::wait(1000); + gps.baud(57600); + WARN("Disabling all unused NMEA sentences"); + gps.printf("$PUBX,40,GLL,0,0,0,0,0,0*5C\r\n"); + gps.printf("$PUBX,40,ZDA,0,0,0,0,0,0*44\r\n"); + gps.printf("$PUBX,40,VTG,0,0,0,0,0,0*5E\r\n"); + gps.printf("$PUBX,40,GSV,0,0,0,0,0,0*59\r\n"); + gps.printf("$PUBX,40,GSA,0,0,0,0,0,0*4E\r\n"); + //gps.printf("$PUBX,40,RMC,0,0,0,0,0,0*47\r\n"); + Thread::wait(500); + gps.rxBufferFlush(); + gps.txBufferFlush(); + WARN("Setup Callbacks"); + gps.attach(&rxCallback, MODSERIAL::RxIrq); + gps.attach(&rxFullCallback, MODSERIAL::RxOvIrq); + InitResults(SENSOR_RESSZ_DEFAULT); + WARN("Finish Init"); +} + +// Could we have a way to put the InitResultStatic/StoreLastImpact into the base class? + +void GPSSensor::InitResultsStatic(void) { + DBG("InitResultStatic have been defined"); + results.start = (uint8_t*)store; + results.current = (uint8_t*)store; + results.max = sizeof(store); //IMU_STORE_SIZE*sizeof(uint16_t); +} + +void GPSSensor::Loop(void) { + // We read all the available data as fast as possible + // but giving opportunity to be preampted (wait 100ms) + // Doing this reading should avoid serial internal buffer to ovf + Sample(); + StoreLastImpact(); + Thread::wait(idleTime); +} + +void GPSSensor::StoreLastImpact(void) { + if(fixed && NeedImpact()) { + if((results.max - results.num)>=sizeof(gpsImpact)){ + DBG("===========> Storing New impact <==========="); + if(resultsMutex.trylock()) { + memcpy((char*)results.current,(const char*)&impact,sizeof(gpsImpact)); + results.current += sizeof(gpsImpact); + results.num += sizeof(gpsImpact); + DBG("%p %d %d",results.current,results.num,sizeof(gpsImpact)); + resultsMutex.unlock(); + } + } else + ERR("No Space to Store results... Data will be overwrited at next Loop"); + } else { + DBG("Nope, (%s)", (!fixed)?"not fixed":"no impact needed"); + } +} + +bool GPSSensor::NeedImpact(void) { + time_t now = time(NULL); + DBG("%ld-%ld = %d > %d ",now,lastImpact,now-lastImpact,trackTime); + if((now-lastImpact)>trackTime){ + DBG("NEED IMPACT"); + lastImpact = now; + return true; + } + return false; +} + +int GPSSensor::GetLine(void) { + int len = 0; + // Consume the chars until we found a '$' + while (gps.getc() != '$'); + recvBuff[0] = '$'; + // Fill the GPS_RECV_BUFF + for (int i=1; i<(GPS_RECV_BUFF-2); i++) { + recvBuff[i] = gps.getc(); + len++; + if (recvBuff[i] == '\n') { + recvBuff[i+1] = '\0'; + return len; + } + } + error("Overflow in getline"); + return GPS_RECV_BUFF-2; +} + +void GPSSensor::Sample(void) { + int len = 0; + bool newData = false; + if(gpsNewlineDetected) { + while(1) { + int nChars = gps.rxBufferGetCount(); + if(nChars == 0) + break; + //else + // DBG("To read --> %d",nChars); + // We could consider passing the data directly to parser whithout buffering it + // but: That's annoying cause if so we could not printf them smoothly on console + len = GetLine(); + + //DBG("GotLine(%d) %s",len,recvBuff); + // DBG_MEMDUMP("GPSData",(const char*)recvBuff,len); + + for(unsigned int i = 0; i < len; i++) { + // Insert all data to gpsParser + if(gpsParser.encode(recvBuff[i])) { + newData = true; + } + } + + // Only take a sample when all the data for each epoch have been received + if(newData && gpsParser.rmc_ready() && gpsParser.gga_ready()){ + long lat = 0, lon = 0, hdop = 0; + unsigned long date = 0, time = 0; + gpsParser.get_position(&lat,&lon); + gpsParser.get_datetime(&date,&time); + hdop = gpsParser.hdop(); + if((lat != TinyGPS::GPS_INVALID_ANGLE) && (lon != TinyGPS::GPS_INVALID_ANGLE)){ + fixed = true; + impact.date = date; + impact.time = time/100; + impact.lat = lat; + impact.lon = lon; + if(hdop>0xffff) { + impact.hdop = -1; + } else { + impact.hdop = hdop; + } + WARN("######[%ld-%ld][%09ld|%09ld|%04d]######",impact.date,impact.time,impact.lat,impact.lon,impact.hdop); + } else { + fixed = false; + DBG("All data have been received but lat/lon does not seems to be valid",date,time,lat,lon,hdop); + } + gpsParser.reset_ready(); + } else { + fixed = false; + 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'); + } + } + } +} + +// OLD SAMPLING +/* +void GPSSensor::Sample(void) { + int lineParsed = 0; + DBG("Read GPS"); + if(gpsNewlineDetected) { + gpsNewlineDetected = false; + while(1) { + int nChars = gps.rxBufferGetCount(); + if(nChars == 0) { + DBG("No data to read anymore on gps line"); + break; + } else { + DBG("--> %d",nChars); + } + + DBG("Read Line GPS"); + GetLine(); + DBG("GotLine, %s",recvBuff); + DBG_MEMDUMP("GPSData",(const char*)recvBuff,strlen((const char*)recvBuff)); + + // Stuffs used in GPGGA + + // The only NMEA sentence we gonna have (all other are disabled) + 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); + if(nArgs == 10) { + DBG("We got all the arguments"); + DBG("%d [%f/%f/%c/%f/%c/%d/%d/%f/%f/%c]\n",nArgs,hour,lat,ns,lon,ew,q,su,hdop,alt,altU); + lineParsed = GGA; + } + + // if lineParsed is different from zero meens that's we got new data + if(lineParsed) { + DBG("Impact Result %d", su); + // When 0 satelites usualy this we can't lock + if(su == 0) + q = 0; + + if(q!=0){ + DBG("Locked (q = %d)",q); + fixed = true; + impact.utc = (uint32_t) hour; + impact.lat = DegToDec(lat,ns); + impact.lon = DegToDec(lon,ew); + impact.alt = alt; + impact.su = su; + impact.hdop = hdop; + DBG("Impact Results %d, %f, %f, %f, %d, %d", impact.utc, impact.lat, impact.lon, impact.alt, impact.su, impact.hdop); + } + } + } + } +} +*/ + \ No newline at end of file