2018/06/18
Diff: GPS.cpp
- Revision:
- 0:2c0d50143d85
- Child:
- 1:2f131675f872
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPS.cpp Thu May 03 14:19:36 2018 +0000 @@ -0,0 +1,220 @@ +#include "mbed.h" +#include "GPS.h" +#include "math.h" +#include "stdio.h" + +GPS::GPS(PinName gpstx,PinName gpsrx): serial(gpstx,gpsrx){ + + gpscmd("314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0"); + int odr=ODR; + odr=1000/odr; + sprintf(buf,"300,%d,0,0,0,0",odr); + gpscmd(buf); + + int baud=GPSBAUD; + + for(int i=0;i<100;i++){ + buf[i]=0; + } + + sprintf(buf,"251,%d",baud); + + gpscmd(buf); + gpscmd(buf); + + + serial.baud(GPSBAUD); + + NVIC_SetPriority(UART2_IRQn,0); + + //m = 1; + //reasonable = false; + + //serial.attach(this,ExtractionData,Serial::RxIrq); + +} + +void GPS::gpscmd(char *command) +{ + int len=strlen(command); + char xordata[len+4]; + sprintf(xordata,"PMTK%s",command); + + + char xorbuf=0x00; + for(int i=0;i<len+4;i++){ + xorbuf=xorbuf^xordata[i]; + } + + + char packet[len+8]; + if(xorbuf>0x0f){ + sprintf(packet,"$%s*%x",xordata,xorbuf); + } + else { + sprintf(packet,"$%s*0%x",xordata,xorbuf); + } + + + serial.printf(packet); + serial.putc(0x0D); + serial.putc(0x0A); +} + +void GPS::attach(void(*fn)()){ + serial.attach(fn, Serial::RxIrq); +} + +void GPS::GetDefault(){ + datadefault = serial.getc(); +} + +void GPS::ExtractionData(){ + + Initialization(); + + datadefault = serial.getc(); + + if(datadefault == '$'){ + + char DATA[20] = "\0"; + Initialization(); + getstring(DATA); + + if(!strcmp(DATA, "GPGGA")){ + //int m; + for(m = 1; m < 6;m++){ + getstring(DATA); + switch(m){ + case 1: + time = strtof(DATA, NULL) + 90000.0; + break; + case 2: + //latitude = strtof(DATA, NULL); + fMinutes = modff(strtof(DATA, NULL), &latitude)*60.0; + latitude = latitude*100.0 + fMinutes; + break; + case 3: + if(!strcmp(DATA, "S")){ + latitude = latitude*-1.0; + } + break; + case 4: + //longtitude = strtof(DATA, NULL); + fMinutes = modff(strtof(DATA, NULL), &longtitude)*60.0; + longtitude = longtitude*100.0 + fMinutes; + break; + case 5: + if(!strcmp(DATA, "W")){ + longtitude = longtitude*-1.0; + } + break; + case 9: + altitude = strtof(DATA, NULL); + break; + case 10: + if(strcmp(DATA, "M")){ + altitude = altitude*-1.0; + } + //m = 1; + readable = true; + break; + } + } + } + } +} + +void GPS::ExtractionData_FULL(){ + + char DATA[20] = "\0"; + + Initialization(); + datadefault = serial.getc(); + + if(datadefault == '$'){ + getstring(DATA); + if(!strcmp(DATA, "GPGGA")){ + reasonable = true; + } + } + + if(reasonable){ + + Initialization(); + getstring(DATA); + + switch(m){ + case 1: + Seconds = modff((strtof(DATA, NULL) + 90000.0)/100.0, &fHours)*100; + fMinutes = modff(fHours/100.0, &fHours)*100; + Hours = fHours; + Minutes = fMinutes; + sprintf(timeFULL, "%d:%d:%f", Hours, Minutes, Seconds); + break; + case 2: + fMinutes = modff(strtof(DATA, NULL)/100.0, &fDegrees)*100.0; + Seconds = modff(strtof(DATA, NULL), &fMinutes)*60.0; + Degrees = fDegrees; + Minutes = fMinutes; + sprintf(latitudeFULL, "%d°%d\'%f\"", Degrees, Minutes, Seconds); + break; + case 3: + sprintf(latitudeFULL, "%s%s", latitudeFULL, DATA); + break; + case 4: + fMinutes = modff(strtof(DATA, NULL)/100.0, &fDegrees)*100.0; + Seconds = modff(strtof(DATA, NULL), &fMinutes)*60.0; + Degrees = fDegrees; + Minutes = fMinutes; + sprintf(longtitudeFULL, "%d°%d\'%f\"", Degrees, Minutes, Seconds); + break; + case 5: + sprintf(longtitudeFULL, "%s%s", longtitudeFULL, DATA); + break; + case 9: + strcpy(altitudeFULL, DATA); + break; + case 10: + sprintf(altitudeFULL, "%s%s", altitudeFULL, DATA); + m = 1; + reasonable = false; + readable = true; + break; + } + + m++; + //sizeof + } +} + +void GPS::getstring(char a[]){ + int n; + for(n = 0;;n++){ + char ch = serial.getc(); + if(ch == ',' ||ch == '\n'){ + a[n] = '\0'; + break; + } + a[n] = ch; + } +} + +void GPS::Initialization(){ + readable = false; + Degrees = -1; + Minutes = -1; + Hours = -1; + longtitude = -1.0; + latitude = -1.0; + altitude = -1.0; + time = -1.0; + fDegrees = -1.0; + fMinutes = -1.0; + Seconds = -1.0; + fHours = -1.0; + longtitudeFULL = "\0"; + latitudeFULL = "\0"; + timeFULL = "\0"; + altitudeFULL = "\0"; +} \ No newline at end of file