Clément BENOIT / PYRN

This my big project for PYRN Board

Dependencies:   CAN HTTPClient MODSERIAL MyThings Pyrn3GModem Socket TinyGPS MyUSBHost lwip-sys lwip mbed-rtos mbed-src

GPSSensor.cpp@0:efe6085327fd, 2015-04-14 (annotated)

Committer:
clemounet
Date:
Tue Apr 14 13:30:02 2015 +0000
Revision:
0:efe6085327fd
All the projet PYRN 3G-CAN-ACC...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
clemounet 0:efe6085327fd 1
clemounet 0:efe6085327fd 2
clemounet 0:efe6085327fd 3 #include "GPSSensor.h"
clemounet 0:efe6085327fd 4
clemounet 0:efe6085327fd 5 #define __DEBUG__ 2
clemounet 0:efe6085327fd 6 #ifndef __MODULE__
clemounet 0:efe6085327fd 7 #define __MODULE__ "GPSSensor.cpp"
clemounet 0:efe6085327fd 8 #endif
clemounet 0:efe6085327fd 9 #include "MyDebug.h"
clemounet 0:efe6085327fd 10
clemounet 0:efe6085327fd 11 #define GPS_THREAD_STACK_SIZE 768
clemounet 0:efe6085327fd 12
clemounet 0:efe6085327fd 13 bool gpsNewlineDetected;
clemounet 0:efe6085327fd 14
clemounet 0:efe6085327fd 15 void rxFullCallback(MODSERIAL_IRQ_INFO *q){
clemounet 0:efe6085327fd 16 DBG("OVF");
clemounet 0:efe6085327fd 17 MODSERIAL *serial = q->serial;
clemounet 0:efe6085327fd 18 serial->rxBufferFlush();
clemounet 0:efe6085327fd 19 }
clemounet 0:efe6085327fd 20
clemounet 0:efe6085327fd 21 void rxCallback(MODSERIAL_IRQ_INFO *q) {
clemounet 0:efe6085327fd 22 MODSERIAL *serial = q->serial;
clemounet 0:efe6085327fd 23 if ( serial->rxGetLastChar() == '\n') {
clemounet 0:efe6085327fd 24 gpsNewlineDetected = true;
clemounet 0:efe6085327fd 25 }
clemounet 0:efe6085327fd 26 }
clemounet 0:efe6085327fd 27
clemounet 0:efe6085327fd 28 // Well there is an issue in the RX Serial data, sometime multiples frames collides between themselves...
clemounet 0:efe6085327fd 29 // To debug when we gonna have GSP Signal
clemounet 0:efe6085327fd 30
clemounet 0:efe6085327fd 31 GPSSensor::GPSSensor(PinName tx, PinName rx, uint32_t trackingTime, uint32_t idle):
clemounet 0:efe6085327fd 32 MySensor(SENSOR_NAME_GPS, SENSOR_TYPE_GPS, idle, GPS_THREAD_STACK_SIZE),
clemounet 0:efe6085327fd 33 gps(tx,rx) {
clemounet 0:efe6085327fd 34 fixed = false;
clemounet 0:efe6085327fd 35 trackTime = trackingTime;
clemounet 0:efe6085327fd 36 lastImpact = 0;
clemounet 0:efe6085327fd 37 gpsNewlineDetected = false;
clemounet 0:efe6085327fd 38 gps.baud(9600);
clemounet 0:efe6085327fd 39 gps.rxBufferFlush();
clemounet 0:efe6085327fd 40 gps.txBufferFlush();
clemounet 0:efe6085327fd 41 WARN("Increase the baudrate to 57600");
clemounet 0:efe6085327fd 42 gps.printf("$PUBX,41,1,0007,0003,57600,0*2B\r\n");
clemounet 0:efe6085327fd 43 Thread::wait(1000);
clemounet 0:efe6085327fd 44 gps.baud(57600);
clemounet 0:efe6085327fd 45 WARN("Disabling all unused NMEA sentences");
clemounet 0:efe6085327fd 46 gps.printf("$PUBX,40,GLL,0,0,0,0,0,0*5C\r\n");
clemounet 0:efe6085327fd 47 gps.printf("$PUBX,40,ZDA,0,0,0,0,0,0*44\r\n");
clemounet 0:efe6085327fd 48 gps.printf("$PUBX,40,VTG,0,0,0,0,0,0*5E\r\n");
clemounet 0:efe6085327fd 49 gps.printf("$PUBX,40,GSV,0,0,0,0,0,0*59\r\n");
clemounet 0:efe6085327fd 50 gps.printf("$PUBX,40,GSA,0,0,0,0,0,0*4E\r\n");
clemounet 0:efe6085327fd 51 //gps.printf("$PUBX,40,RMC,0,0,0,0,0,0*47\r\n");
clemounet 0:efe6085327fd 52 Thread::wait(500);
clemounet 0:efe6085327fd 53 gps.rxBufferFlush();
clemounet 0:efe6085327fd 54 gps.txBufferFlush();
clemounet 0:efe6085327fd 55 WARN("Setup Callbacks");
clemounet 0:efe6085327fd 56 gps.attach(&rxCallback, MODSERIAL::RxIrq);
clemounet 0:efe6085327fd 57 gps.attach(&rxFullCallback, MODSERIAL::RxOvIrq);
clemounet 0:efe6085327fd 58 InitResults(SENSOR_RESSZ_DEFAULT);
clemounet 0:efe6085327fd 59 WARN("Finish Init");
clemounet 0:efe6085327fd 60 }
clemounet 0:efe6085327fd 61
clemounet 0:efe6085327fd 62 // Could we have a way to put the InitResultStatic/StoreLastImpact into the base class?
clemounet 0:efe6085327fd 63
clemounet 0:efe6085327fd 64 void GPSSensor::InitResultsStatic(void) {
clemounet 0:efe6085327fd 65 DBG("InitResultStatic have been defined");
clemounet 0:efe6085327fd 66 results.start = (uint8_t*)store;
clemounet 0:efe6085327fd 67 results.current = (uint8_t*)store;
clemounet 0:efe6085327fd 68 results.max = sizeof(store); //IMU_STORE_SIZE*sizeof(uint16_t);
clemounet 0:efe6085327fd 69 }
clemounet 0:efe6085327fd 70
clemounet 0:efe6085327fd 71 void GPSSensor::Loop(void) {
clemounet 0:efe6085327fd 72 // We read all the available data as fast as possible
clemounet 0:efe6085327fd 73 // but giving opportunity to be preampted (wait 100ms)
clemounet 0:efe6085327fd 74 // Doing this reading should avoid serial internal buffer to ovf
clemounet 0:efe6085327fd 75 Sample();
clemounet 0:efe6085327fd 76 StoreLastImpact();
clemounet 0:efe6085327fd 77 Thread::wait(idleTime);
clemounet 0:efe6085327fd 78 }
clemounet 0:efe6085327fd 79
clemounet 0:efe6085327fd 80 void GPSSensor::StoreLastImpact(void) {
clemounet 0:efe6085327fd 81 if(fixed && NeedImpact()) {
clemounet 0:efe6085327fd 82 if((results.max - results.num)>=sizeof(gpsImpact)){
clemounet 0:efe6085327fd 83 DBG("===========> Storing New impact <===========");
clemounet 0:efe6085327fd 84 if(resultsMutex.trylock()) {
clemounet 0:efe6085327fd 85 memcpy((char*)results.current,(const char*)&impact,sizeof(gpsImpact));
clemounet 0:efe6085327fd 86 results.current += sizeof(gpsImpact);
clemounet 0:efe6085327fd 87 results.num += sizeof(gpsImpact);
clemounet 0:efe6085327fd 88 DBG("%p %d %d",results.current,results.num,sizeof(gpsImpact));
clemounet 0:efe6085327fd 89 resultsMutex.unlock();
clemounet 0:efe6085327fd 90 }
clemounet 0:efe6085327fd 91 } else
clemounet 0:efe6085327fd 92 ERR("No Space to Store results... Data will be overwrited at next Loop");
clemounet 0:efe6085327fd 93 } else {
clemounet 0:efe6085327fd 94 DBG("Nope, (%s)", (!fixed)?"not fixed":"no impact needed");
clemounet 0:efe6085327fd 95 }
clemounet 0:efe6085327fd 96 }
clemounet 0:efe6085327fd 97
clemounet 0:efe6085327fd 98 bool GPSSensor::NeedImpact(void) {
clemounet 0:efe6085327fd 99 time_t now = time(NULL);
clemounet 0:efe6085327fd 100 DBG("%ld-%ld = %d > %d ",now,lastImpact,now-lastImpact,trackTime);
clemounet 0:efe6085327fd 101 if((now-lastImpact)>trackTime){
clemounet 0:efe6085327fd 102 DBG("NEED IMPACT");
clemounet 0:efe6085327fd 103 lastImpact = now;
clemounet 0:efe6085327fd 104 return true;
clemounet 0:efe6085327fd 105 }
clemounet 0:efe6085327fd 106 return false;
clemounet 0:efe6085327fd 107 }
clemounet 0:efe6085327fd 108
clemounet 0:efe6085327fd 109 int GPSSensor::GetLine(void) {
clemounet 0:efe6085327fd 110 int len = 0;
clemounet 0:efe6085327fd 111 // Consume the chars until we found a '$'
clemounet 0:efe6085327fd 112 while (gps.getc() != '$');
clemounet 0:efe6085327fd 113 recvBuff[0] = '$';
clemounet 0:efe6085327fd 114 // Fill the GPS_RECV_BUFF
clemounet 0:efe6085327fd 115 for (int i=1; i<(GPS_RECV_BUFF-2); i++) {
clemounet 0:efe6085327fd 116 recvBuff[i] = gps.getc();
clemounet 0:efe6085327fd 117 len++;
clemounet 0:efe6085327fd 118 if (recvBuff[i] == '\n') {
clemounet 0:efe6085327fd 119 recvBuff[i+1] = '\0';
clemounet 0:efe6085327fd 120 return len;
clemounet 0:efe6085327fd 121 }
clemounet 0:efe6085327fd 122 }
clemounet 0:efe6085327fd 123 error("Overflow in getline");
clemounet 0:efe6085327fd 124 return GPS_RECV_BUFF-2;
clemounet 0:efe6085327fd 125 }
clemounet 0:efe6085327fd 126
clemounet 0:efe6085327fd 127 void GPSSensor::Sample(void) {
clemounet 0:efe6085327fd 128 int len = 0;
clemounet 0:efe6085327fd 129 bool newData = false;
clemounet 0:efe6085327fd 130 if(gpsNewlineDetected) {
clemounet 0:efe6085327fd 131 while(1) {
clemounet 0:efe6085327fd 132 int nChars = gps.rxBufferGetCount();
clemounet 0:efe6085327fd 133 if(nChars == 0)
clemounet 0:efe6085327fd 134 break;
clemounet 0:efe6085327fd 135 //else
clemounet 0:efe6085327fd 136 // DBG("To read --> %d",nChars);
clemounet 0:efe6085327fd 137 // We could consider passing the data directly to parser whithout buffering it
clemounet 0:efe6085327fd 138 // but: That's annoying cause if so we could not printf them smoothly on console
clemounet 0:efe6085327fd 139 len = GetLine();
clemounet 0:efe6085327fd 140
clemounet 0:efe6085327fd 141 //DBG("GotLine(%d) %s",len,recvBuff);
clemounet 0:efe6085327fd 142 // DBG_MEMDUMP("GPSData",(const char*)recvBuff,len);
clemounet 0:efe6085327fd 143
clemounet 0:efe6085327fd 144 for(unsigned int i = 0; i < len; i++) {
clemounet 0:efe6085327fd 145 // Insert all data to gpsParser
clemounet 0:efe6085327fd 146 if(gpsParser.encode(recvBuff[i])) {
clemounet 0:efe6085327fd 147 newData = true;
clemounet 0:efe6085327fd 148 }
clemounet 0:efe6085327fd 149 }
clemounet 0:efe6085327fd 150
clemounet 0:efe6085327fd 151 // Only take a sample when all the data for each epoch have been received
clemounet 0:efe6085327fd 152 if(newData && gpsParser.rmc_ready() && gpsParser.gga_ready()){
clemounet 0:efe6085327fd 153 long lat = 0, lon = 0, hdop = 0;
clemounet 0:efe6085327fd 154 unsigned long date = 0, time = 0;
clemounet 0:efe6085327fd 155 gpsParser.get_position(&lat,&lon);
clemounet 0:efe6085327fd 156 gpsParser.get_datetime(&date,&time);
clemounet 0:efe6085327fd 157 hdop = gpsParser.hdop();
clemounet 0:efe6085327fd 158 if((lat != TinyGPS::GPS_INVALID_ANGLE) && (lon != TinyGPS::GPS_INVALID_ANGLE)){
clemounet 0:efe6085327fd 159 fixed = true;
clemounet 0:efe6085327fd 160 impact.date = date;
clemounet 0:efe6085327fd 161 impact.time = time/100;
clemounet 0:efe6085327fd 162 impact.lat = lat;
clemounet 0:efe6085327fd 163 impact.lon = lon;
clemounet 0:efe6085327fd 164 if(hdop>0xffff) {
clemounet 0:efe6085327fd 165 impact.hdop = -1;
clemounet 0:efe6085327fd 166 } else {
clemounet 0:efe6085327fd 167 impact.hdop = hdop;
clemounet 0:efe6085327fd 168 }
clemounet 0:efe6085327fd 169 WARN("######[%ld-%ld][%09ld|%09ld|%04d]######",impact.date,impact.time,impact.lat,impact.lon,impact.hdop);
clemounet 0:efe6085327fd 170 } else {
clemounet 0:efe6085327fd 171 fixed = false;
clemounet 0:efe6085327fd 172 DBG("All data have been received but lat/lon does not seems to be valid",date,time,lat,lon,hdop);
clemounet 0:efe6085327fd 173 }
clemounet 0:efe6085327fd 174 gpsParser.reset_ready();
clemounet 0:efe6085327fd 175 } else {
clemounet 0:efe6085327fd 176 fixed = false;
clemounet 0:efe6085327fd 177 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');
clemounet 0:efe6085327fd 178 }
clemounet 0:efe6085327fd 179 }
clemounet 0:efe6085327fd 180 }
clemounet 0:efe6085327fd 181 }
clemounet 0:efe6085327fd 182
clemounet 0:efe6085327fd 183 // OLD SAMPLING
clemounet 0:efe6085327fd 184 /*
clemounet 0:efe6085327fd 185 void GPSSensor::Sample(void) {
clemounet 0:efe6085327fd 186 int lineParsed = 0;
clemounet 0:efe6085327fd 187 DBG("Read GPS");
clemounet 0:efe6085327fd 188 if(gpsNewlineDetected) {
clemounet 0:efe6085327fd 189 gpsNewlineDetected = false;
clemounet 0:efe6085327fd 190 while(1) {
clemounet 0:efe6085327fd 191 int nChars = gps.rxBufferGetCount();
clemounet 0:efe6085327fd 192 if(nChars == 0) {
clemounet 0:efe6085327fd 193 DBG("No data to read anymore on gps line");
clemounet 0:efe6085327fd 194 break;
clemounet 0:efe6085327fd 195 } else {
clemounet 0:efe6085327fd 196 DBG("--> %d",nChars);
clemounet 0:efe6085327fd 197 }
clemounet 0:efe6085327fd 198
clemounet 0:efe6085327fd 199 DBG("Read Line GPS");
clemounet 0:efe6085327fd 200 GetLine();
clemounet 0:efe6085327fd 201 DBG("GotLine, %s",recvBuff);
clemounet 0:efe6085327fd 202 DBG_MEMDUMP("GPSData",(const char*)recvBuff,strlen((const char*)recvBuff));
clemounet 0:efe6085327fd 203
clemounet 0:efe6085327fd 204 // Stuffs used in GPGGA
clemounet 0:efe6085327fd 205
clemounet 0:efe6085327fd 206 // The only NMEA sentence we gonna have (all other are disabled)
clemounet 0:efe6085327fd 207 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);
clemounet 0:efe6085327fd 208 if(nArgs == 10) {
clemounet 0:efe6085327fd 209 DBG("We got all the arguments");
clemounet 0:efe6085327fd 210 DBG("%d [%f/%f/%c/%f/%c/%d/%d/%f/%f/%c]\n",nArgs,hour,lat,ns,lon,ew,q,su,hdop,alt,altU);
clemounet 0:efe6085327fd 211 lineParsed = GGA;
clemounet 0:efe6085327fd 212 }
clemounet 0:efe6085327fd 213
clemounet 0:efe6085327fd 214 // if lineParsed is different from zero meens that's we got new data
clemounet 0:efe6085327fd 215 if(lineParsed) {
clemounet 0:efe6085327fd 216 DBG("Impact Result %d", su);
clemounet 0:efe6085327fd 217 // When 0 satelites usualy this we can't lock
clemounet 0:efe6085327fd 218 if(su == 0)
clemounet 0:efe6085327fd 219 q = 0;
clemounet 0:efe6085327fd 220
clemounet 0:efe6085327fd 221 if(q!=0){
clemounet 0:efe6085327fd 222 DBG("Locked (q = %d)",q);
clemounet 0:efe6085327fd 223 fixed = true;
clemounet 0:efe6085327fd 224 impact.utc = (uint32_t) hour;
clemounet 0:efe6085327fd 225 impact.lat = DegToDec(lat,ns);
clemounet 0:efe6085327fd 226 impact.lon = DegToDec(lon,ew);
clemounet 0:efe6085327fd 227 impact.alt = alt;
clemounet 0:efe6085327fd 228 impact.su = su;
clemounet 0:efe6085327fd 229 impact.hdop = hdop;
clemounet 0:efe6085327fd 230 DBG("Impact Results %d, %f, %f, %f, %d, %d", impact.utc, impact.lat, impact.lon, impact.alt, impact.su, impact.hdop);
clemounet 0:efe6085327fd 231 }
clemounet 0:efe6085327fd 232 }
clemounet 0:efe6085327fd 233 }
clemounet 0:efe6085327fd 234 }
clemounet 0:efe6085327fd 235 }
clemounet 0:efe6085327fd 236 */
clemounet 0:efe6085327fd 237