Mbed library to get location informatiuon from GYSFDMAXB
GPS.cpp@2:bd69260c2bd1, 2016-07-25 (annotated)
- Committer:
- kim1212
- Date:
- Mon Jul 25 07:40:37 2016 +0000
- Revision:
- 2:bd69260c2bd1
- Parent:
- 1:68601d2af8a5
?????????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kim1212 | 0:4cc88ba7719b | 1 | #include "mbed.h" |
kim1212 | 0:4cc88ba7719b | 2 | #include "GPS.h" |
kim1212 | 0:4cc88ba7719b | 3 | |
kim1212 | 0:4cc88ba7719b | 4 | GPS::GPS(PinName gpstx,PinName gpsrx): _gps(gpstx,gpsrx) |
kim1212 | 0:4cc88ba7719b | 5 | { |
kim1212 | 1:68601d2af8a5 | 6 | latitude=0; |
kim1212 | 1:68601d2af8a5 | 7 | longtitude=0; |
kim1212 | 1:68601d2af8a5 | 8 | |
kim1212 | 0:4cc88ba7719b | 9 | gpscmd("314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0"); |
kim1212 | 0:4cc88ba7719b | 10 | int odr=ODR; |
kim1212 | 0:4cc88ba7719b | 11 | odr=1000/odr; |
kim1212 | 0:4cc88ba7719b | 12 | sprintf(buf,"300,%d,0,0,0,0",odr); |
kim1212 | 0:4cc88ba7719b | 13 | gpscmd(buf); |
kim1212 | 0:4cc88ba7719b | 14 | |
kim1212 | 0:4cc88ba7719b | 15 | int baud=GPSBAUD; |
kim1212 | 0:4cc88ba7719b | 16 | |
kim1212 | 0:4cc88ba7719b | 17 | for(int i=0;i<100;i++){ |
kim1212 | 0:4cc88ba7719b | 18 | buf[i]=0; |
kim1212 | 0:4cc88ba7719b | 19 | } |
kim1212 | 0:4cc88ba7719b | 20 | |
kim1212 | 0:4cc88ba7719b | 21 | sprintf(buf,"251,%d",baud); |
kim1212 | 0:4cc88ba7719b | 22 | |
kim1212 | 0:4cc88ba7719b | 23 | gpscmd(buf); |
kim1212 | 0:4cc88ba7719b | 24 | gpscmd(buf); |
kim1212 | 0:4cc88ba7719b | 25 | |
kim1212 | 0:4cc88ba7719b | 26 | |
kim1212 | 0:4cc88ba7719b | 27 | _gps.baud(GPSBAUD); |
kim1212 | 0:4cc88ba7719b | 28 | |
kim1212 | 0:4cc88ba7719b | 29 | _gps.attach(this,&GPS::getgps); |
kim1212 | 0:4cc88ba7719b | 30 | } |
kim1212 | 0:4cc88ba7719b | 31 | |
kim1212 | 0:4cc88ba7719b | 32 | void GPS::gpscmd(char *command) |
kim1212 | 0:4cc88ba7719b | 33 | { |
kim1212 | 0:4cc88ba7719b | 34 | int len=strlen(command); |
kim1212 | 0:4cc88ba7719b | 35 | char xordata[len+4]; |
kim1212 | 0:4cc88ba7719b | 36 | sprintf(xordata,"PMTK%s",command); |
kim1212 | 0:4cc88ba7719b | 37 | |
kim1212 | 0:4cc88ba7719b | 38 | |
kim1212 | 0:4cc88ba7719b | 39 | char xorbuf=0x00; |
kim1212 | 0:4cc88ba7719b | 40 | for(int i=0;i<len+4;i++){ |
kim1212 | 0:4cc88ba7719b | 41 | xorbuf=xorbuf^xordata[i]; |
kim1212 | 0:4cc88ba7719b | 42 | } |
kim1212 | 0:4cc88ba7719b | 43 | |
kim1212 | 0:4cc88ba7719b | 44 | |
kim1212 | 0:4cc88ba7719b | 45 | char packet[len+8]; |
kim1212 | 0:4cc88ba7719b | 46 | if(xorbuf>0x0f){ |
kim1212 | 0:4cc88ba7719b | 47 | sprintf(packet,"$%s*%x",xordata,xorbuf); |
kim1212 | 0:4cc88ba7719b | 48 | } |
kim1212 | 0:4cc88ba7719b | 49 | else { |
kim1212 | 0:4cc88ba7719b | 50 | sprintf(packet,"$%s*0%x",xordata,xorbuf); |
kim1212 | 0:4cc88ba7719b | 51 | } |
kim1212 | 0:4cc88ba7719b | 52 | |
kim1212 | 0:4cc88ba7719b | 53 | |
kim1212 | 0:4cc88ba7719b | 54 | _gps.printf(packet); |
kim1212 | 0:4cc88ba7719b | 55 | _gps.putc(0x0D); |
kim1212 | 0:4cc88ba7719b | 56 | _gps.putc(0x0A); |
kim1212 | 0:4cc88ba7719b | 57 | } |
kim1212 | 0:4cc88ba7719b | 58 | |
kim1212 | 0:4cc88ba7719b | 59 | void GPS::getgps() |
kim1212 | 0:4cc88ba7719b | 60 | { |
kim1212 | 0:4cc88ba7719b | 61 | if(_gps.getc()=='$'){ |
kim1212 | 0:4cc88ba7719b | 62 | |
kim1212 | 0:4cc88ba7719b | 63 | int counter; |
kim1212 | 0:4cc88ba7719b | 64 | char sample=0x00; |
kim1212 | 0:4cc88ba7719b | 65 | |
kim1212 | 2:bd69260c2bd1 | 66 | |
kim1212 | 2:bd69260c2bd1 | 67 | |
kim1212 | 2:bd69260c2bd1 | 68 | |
kim1212 | 0:4cc88ba7719b | 69 | |
kim1212 | 0:4cc88ba7719b | 70 | |
kim1212 | 0:4cc88ba7719b | 71 | for(counter=0;counter<18;counter++){ |
kim1212 | 0:4cc88ba7719b | 72 | sample=_gps.getc(); |
kim1212 | 0:4cc88ba7719b | 73 | } |
kim1212 | 0:4cc88ba7719b | 74 | |
kim1212 | 0:4cc88ba7719b | 75 | if(sample=='A'){ |
kim1212 | 0:4cc88ba7719b | 76 | |
kim1212 | 0:4cc88ba7719b | 77 | //初期化 |
kim1212 | 0:4cc88ba7719b | 78 | latitude=0; |
kim1212 | 0:4cc88ba7719b | 79 | longtitude=0; |
kim1212 | 2:bd69260c2bd1 | 80 | speed=0; |
kim1212 | 2:bd69260c2bd1 | 81 | direction=0; |
kim1212 | 0:4cc88ba7719b | 82 | |
kim1212 | 0:4cc88ba7719b | 83 | //データ取得 |
kim1212 | 2:bd69260c2bd1 | 84 | for(int i=0;i<4;i++){ |
kim1212 | 2:bd69260c2bd1 | 85 | point[i]=0; |
kim1212 | 2:bd69260c2bd1 | 86 | } |
kim1212 | 2:bd69260c2bd1 | 87 | |
kim1212 | 2:bd69260c2bd1 | 88 | int pointnum=0; |
kim1212 | 0:4cc88ba7719b | 89 | for(counter=0;sample!='\n';counter++){ |
kim1212 | 0:4cc88ba7719b | 90 | sample=_gps.getc(); |
kim1212 | 0:4cc88ba7719b | 91 | buf[counter]=sample; |
kim1212 | 0:4cc88ba7719b | 92 | |
kim1212 | 2:bd69260c2bd1 | 93 | if(buf[counter]=='.'){ |
kim1212 | 2:bd69260c2bd1 | 94 | point[pointnum]=counter; |
kim1212 | 2:bd69260c2bd1 | 95 | pointnum++; |
kim1212 | 0:4cc88ba7719b | 96 | } |
kim1212 | 0:4cc88ba7719b | 97 | |
kim1212 | 0:4cc88ba7719b | 98 | } |
kim1212 | 0:4cc88ba7719b | 99 | |
kim1212 | 0:4cc88ba7719b | 100 | //緯度計算 |
kim1212 | 2:bd69260c2bd1 | 101 | for(counter=1;counter<point[0]-2;counter++){ |
kim1212 | 0:4cc88ba7719b | 102 | |
kim1212 | 0:4cc88ba7719b | 103 | float num=1; |
kim1212 | 2:bd69260c2bd1 | 104 | for(int i=point[0]-counter-3;i>0;i--){ |
kim1212 | 0:4cc88ba7719b | 105 | num=num*10; |
kim1212 | 0:4cc88ba7719b | 106 | } |
kim1212 | 0:4cc88ba7719b | 107 | |
kim1212 | 0:4cc88ba7719b | 108 | latitude+=(float)num*(buf[counter]-48); |
kim1212 | 0:4cc88ba7719b | 109 | } |
kim1212 | 0:4cc88ba7719b | 110 | |
kim1212 | 0:4cc88ba7719b | 111 | float minute=0; |
kim1212 | 0:4cc88ba7719b | 112 | |
kim1212 | 2:bd69260c2bd1 | 113 | minute+=(buf[point[0]-2]-48)*10; |
kim1212 | 2:bd69260c2bd1 | 114 | minute+=(buf[point[0]-1]-48); |
kim1212 | 0:4cc88ba7719b | 115 | |
kim1212 | 2:bd69260c2bd1 | 116 | for(counter=point[0]+1;counter<point[0]+5;counter++){ |
kim1212 | 0:4cc88ba7719b | 117 | float num=1; |
kim1212 | 0:4cc88ba7719b | 118 | |
kim1212 | 2:bd69260c2bd1 | 119 | for(int i=counter-point[0];i>0;i--){ |
kim1212 | 1:68601d2af8a5 | 120 | num=num*0.1f; |
kim1212 | 0:4cc88ba7719b | 121 | } |
kim1212 | 0:4cc88ba7719b | 122 | |
kim1212 | 0:4cc88ba7719b | 123 | minute+=(float)num*(buf[counter]-48); |
kim1212 | 0:4cc88ba7719b | 124 | } |
kim1212 | 0:4cc88ba7719b | 125 | |
kim1212 | 0:4cc88ba7719b | 126 | latitude+=minute/60; |
kim1212 | 0:4cc88ba7719b | 127 | |
kim1212 | 0:4cc88ba7719b | 128 | |
kim1212 | 0:4cc88ba7719b | 129 | //経度計算 |
kim1212 | 2:bd69260c2bd1 | 130 | for(counter=point[0]+8;counter<point[1]-2;counter++){ |
kim1212 | 0:4cc88ba7719b | 131 | |
kim1212 | 0:4cc88ba7719b | 132 | float num=1; |
kim1212 | 2:bd69260c2bd1 | 133 | for(int i=point[1]-counter-3;i>0;i--){ |
kim1212 | 0:4cc88ba7719b | 134 | num=num*10; |
kim1212 | 0:4cc88ba7719b | 135 | } |
kim1212 | 0:4cc88ba7719b | 136 | |
kim1212 | 0:4cc88ba7719b | 137 | longtitude+=(float)num*(buf[counter]-48); |
kim1212 | 0:4cc88ba7719b | 138 | } |
kim1212 | 0:4cc88ba7719b | 139 | |
kim1212 | 0:4cc88ba7719b | 140 | minute=0; |
kim1212 | 0:4cc88ba7719b | 141 | |
kim1212 | 2:bd69260c2bd1 | 142 | minute+=(buf[point[1]-2]-48)*10; |
kim1212 | 2:bd69260c2bd1 | 143 | minute+=(buf[point[1]-1]-48); |
kim1212 | 0:4cc88ba7719b | 144 | |
kim1212 | 2:bd69260c2bd1 | 145 | for(counter=point[1]+1;counter<point[1]+5;counter++){ |
kim1212 | 0:4cc88ba7719b | 146 | float num=1; |
kim1212 | 0:4cc88ba7719b | 147 | |
kim1212 | 2:bd69260c2bd1 | 148 | for(int i=counter-point[1];i>0;i--){ |
kim1212 | 1:68601d2af8a5 | 149 | num=num*0.1f; |
kim1212 | 0:4cc88ba7719b | 150 | } |
kim1212 | 0:4cc88ba7719b | 151 | |
kim1212 | 0:4cc88ba7719b | 152 | minute+=(float)num*(buf[counter]-48); |
kim1212 | 0:4cc88ba7719b | 153 | } |
kim1212 | 0:4cc88ba7719b | 154 | |
kim1212 | 0:4cc88ba7719b | 155 | longtitude+=minute/60; |
kim1212 | 0:4cc88ba7719b | 156 | |
kim1212 | 0:4cc88ba7719b | 157 | |
kim1212 | 2:bd69260c2bd1 | 158 | //速度計算 |
kim1212 | 2:bd69260c2bd1 | 159 | for(counter=point[1]+9;counter<point[1];counter++){ |
kim1212 | 2:bd69260c2bd1 | 160 | |
kim1212 | 2:bd69260c2bd1 | 161 | float num=1; |
kim1212 | 2:bd69260c2bd1 | 162 | for(int i=point[2]-counter-1;i>0;i--){ |
kim1212 | 2:bd69260c2bd1 | 163 | num=num*10; |
kim1212 | 2:bd69260c2bd1 | 164 | } |
kim1212 | 2:bd69260c2bd1 | 165 | |
kim1212 | 2:bd69260c2bd1 | 166 | speed+=(float)num*(buf[counter]-48); |
kim1212 | 2:bd69260c2bd1 | 167 | } |
kim1212 | 2:bd69260c2bd1 | 168 | |
kim1212 | 2:bd69260c2bd1 | 169 | |
kim1212 | 2:bd69260c2bd1 | 170 | speed+=(buf[point[2]+2]-48)*0.01; |
kim1212 | 2:bd69260c2bd1 | 171 | speed+=(buf[point[2]+1]-48)*0.1; |
kim1212 | 2:bd69260c2bd1 | 172 | |
kim1212 | 2:bd69260c2bd1 | 173 | speed/=0.51444; |
kim1212 | 2:bd69260c2bd1 | 174 | |
kim1212 | 2:bd69260c2bd1 | 175 | //方位計算 |
kim1212 | 2:bd69260c2bd1 | 176 | for(counter=point[2]+4;counter<point[3];counter++){ |
kim1212 | 2:bd69260c2bd1 | 177 | |
kim1212 | 2:bd69260c2bd1 | 178 | float num=1; |
kim1212 | 2:bd69260c2bd1 | 179 | for(int i=point[3]-counter-1;i>0;i--){ |
kim1212 | 2:bd69260c2bd1 | 180 | num=num*10; |
kim1212 | 2:bd69260c2bd1 | 181 | } |
kim1212 | 2:bd69260c2bd1 | 182 | |
kim1212 | 2:bd69260c2bd1 | 183 | direction+=(float)num*(buf[counter]-48); |
kim1212 | 2:bd69260c2bd1 | 184 | } |
kim1212 | 2:bd69260c2bd1 | 185 | |
kim1212 | 2:bd69260c2bd1 | 186 | |
kim1212 | 2:bd69260c2bd1 | 187 | direction+=(buf[point[3]+2]-48)*0.01; |
kim1212 | 2:bd69260c2bd1 | 188 | direction+=(buf[point[3]+1]-48)*0.1; |
kim1212 | 2:bd69260c2bd1 | 189 | |
kim1212 | 2:bd69260c2bd1 | 190 | |
kim1212 | 2:bd69260c2bd1 | 191 | |
kim1212 | 0:4cc88ba7719b | 192 | result=true; |
kim1212 | 0:4cc88ba7719b | 193 | } |
kim1212 | 0:4cc88ba7719b | 194 | |
kim1212 | 0:4cc88ba7719b | 195 | else{ |
kim1212 | 0:4cc88ba7719b | 196 | result=false; |
kim1212 | 0:4cc88ba7719b | 197 | } |
kim1212 | 0:4cc88ba7719b | 198 | |
kim1212 | 0:4cc88ba7719b | 199 | |
kim1212 | 0:4cc88ba7719b | 200 | |
kim1212 | 0:4cc88ba7719b | 201 | |
kim1212 | 0:4cc88ba7719b | 202 | |
kim1212 | 0:4cc88ba7719b | 203 | } |
kim1212 | 0:4cc88ba7719b | 204 | } |