Mbed library to get location informatiuon from GYSFDMAXB
Diff: GPS.cpp
- Revision:
- 0:4cc88ba7719b
- Child:
- 1:68601d2af8a5
diff -r 000000000000 -r 4cc88ba7719b GPS.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPS.cpp Mon Apr 11 04:14:16 2016 +0000 @@ -0,0 +1,161 @@ +#include "mbed.h" +#include "GPS.h" + +GPS::GPS(PinName gpstx,PinName gpsrx): _gps(gpstx,gpsrx) +{ + gpscmd("314,0,1,0,0,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); + + + _gps.baud(GPSBAUD); + + _gps.attach(this,&GPS::getgps); +} + +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); + } + + + _gps.printf(packet); + _gps.putc(0x0D); + _gps.putc(0x0A); +} + +void GPS::getgps() +{ + if(_gps.getc()=='$'){ + + int counter; + char sample=0x00; + + int point1=0; + int point2=0; + + + for(counter=0;counter<18;counter++){ + sample=_gps.getc(); + } + + if(sample=='A'){ + + //初期化 + latitude=0; + longtitude=0; + + //データ取得 + for(counter=0;sample!='\n';counter++){ + sample=_gps.getc(); + buf[counter]=sample; + + if(buf[counter]=='.'&&point1==0){ + point1=counter; + } + + if(counter>8&&buf[counter]=='.'&&point2==0){ + point2=counter; + } + } + + //緯度計算 + for(counter=1;counter<point1-2;counter++){ + + float num=1; + for(int i=point1-counter-3;i>0;i--){ + num=num*10; + } + + latitude+=(float)num*(buf[counter]-48); + } + + float minute=0; + + minute+=(buf[point1-2]-48)*10; + minute+=(buf[point1-1]-48); + + for(counter=point1+1;counter<point1+5;counter++){ + float num=1; + + for(int i=counter-point1;i>0;i--){ + num=num*0.1; + } + + minute+=(float)num*(buf[counter]-48); + } + + latitude+=minute/60; + + + //経度計算 + for(counter=point1+8;counter<point2-2;counter++){ + + float num=1; + for(int i=point2-counter-3;i>0;i--){ + num=num*10; + } + + longtitude+=(float)num*(buf[counter]-48); + } + + minute=0; + + minute+=(buf[point2-2]-48)*10; + minute+=(buf[point2-1]-48); + + for(counter=point2+1;counter<point2+5;counter++){ + float num=1; + + for(int i=counter-point2;i>0;i--){ + num=num*0.1; + } + + minute+=(float)num*(buf[counter]-48); + } + + longtitude+=minute/60; + + + result=true; + } + + else{ + result=false; + } + + + + + + } +} \ No newline at end of file