Tomo Suzu / GPS

2018/06/18

Dependents:   library

GPS.cpp@1:2f131675f872, 2018-05-04 (annotated)

Committer:
Tomo073
Date:
Fri May 04 15:16:02 2018 +0000
Revision:
1:2f131675f872
Parent:
0:2c0d50143d85
Child:
2:578f518a6a05
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tomo073 0:2c0d50143d85 1 #include "mbed.h"
Tomo073 0:2c0d50143d85 2 #include "GPS.h"
Tomo073 0:2c0d50143d85 3 #include "math.h"
Tomo073 0:2c0d50143d85 4 #include "stdio.h"
Tomo073 0:2c0d50143d85 5
Tomo073 0:2c0d50143d85 6 GPS::GPS(PinName gpstx,PinName gpsrx): serial(gpstx,gpsrx){
Tomo073 0:2c0d50143d85 7
Tomo073 0:2c0d50143d85 8 gpscmd("314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0");
Tomo073 0:2c0d50143d85 9 int odr=ODR;
Tomo073 0:2c0d50143d85 10 odr=1000/odr;
Tomo073 0:2c0d50143d85 11 sprintf(buf,"300,%d,0,0,0,0",odr);
Tomo073 0:2c0d50143d85 12 gpscmd(buf);
Tomo073 0:2c0d50143d85 13
Tomo073 0:2c0d50143d85 14 int baud=GPSBAUD;
Tomo073 0:2c0d50143d85 15
Tomo073 0:2c0d50143d85 16 for(int i=0;i<100;i++){
Tomo073 0:2c0d50143d85 17 buf[i]=0;
Tomo073 0:2c0d50143d85 18 }
Tomo073 0:2c0d50143d85 19
Tomo073 0:2c0d50143d85 20 sprintf(buf,"251,%d",baud);
Tomo073 0:2c0d50143d85 21
Tomo073 0:2c0d50143d85 22 gpscmd(buf);
Tomo073 0:2c0d50143d85 23 gpscmd(buf);
Tomo073 0:2c0d50143d85 24
Tomo073 0:2c0d50143d85 25
Tomo073 0:2c0d50143d85 26 serial.baud(GPSBAUD);
Tomo073 0:2c0d50143d85 27
Tomo073 0:2c0d50143d85 28 NVIC_SetPriority(UART2_IRQn,0);
Tomo073 0:2c0d50143d85 29
Tomo073 1:2f131675f872 30 info = 1;
Tomo073 1:2f131675f872 31 reasonable = false;
Tomo073 0:2c0d50143d85 32
Tomo073 0:2c0d50143d85 33 //serial.attach(this,ExtractionData,Serial::RxIrq);
Tomo073 0:2c0d50143d85 34
Tomo073 0:2c0d50143d85 35 }
Tomo073 0:2c0d50143d85 36
Tomo073 0:2c0d50143d85 37 void GPS::gpscmd(char *command)
Tomo073 0:2c0d50143d85 38 {
Tomo073 0:2c0d50143d85 39 int len=strlen(command);
Tomo073 0:2c0d50143d85 40 char xordata[len+4];
Tomo073 0:2c0d50143d85 41 sprintf(xordata,"PMTK%s",command);
Tomo073 0:2c0d50143d85 42
Tomo073 0:2c0d50143d85 43
Tomo073 0:2c0d50143d85 44 char xorbuf=0x00;
Tomo073 0:2c0d50143d85 45 for(int i=0;i<len+4;i++){
Tomo073 0:2c0d50143d85 46 xorbuf=xorbuf^xordata[i];
Tomo073 0:2c0d50143d85 47 }
Tomo073 0:2c0d50143d85 48
Tomo073 0:2c0d50143d85 49
Tomo073 0:2c0d50143d85 50 char packet[len+8];
Tomo073 0:2c0d50143d85 51 if(xorbuf>0x0f){
Tomo073 0:2c0d50143d85 52 sprintf(packet,"$%s*%x",xordata,xorbuf);
Tomo073 0:2c0d50143d85 53 }
Tomo073 0:2c0d50143d85 54 else {
Tomo073 0:2c0d50143d85 55 sprintf(packet,"$%s*0%x",xordata,xorbuf);
Tomo073 0:2c0d50143d85 56 }
Tomo073 0:2c0d50143d85 57
Tomo073 0:2c0d50143d85 58
Tomo073 0:2c0d50143d85 59 serial.printf(packet);
Tomo073 0:2c0d50143d85 60 serial.putc(0x0D);
Tomo073 0:2c0d50143d85 61 serial.putc(0x0A);
Tomo073 0:2c0d50143d85 62 }
Tomo073 0:2c0d50143d85 63
Tomo073 0:2c0d50143d85 64 void GPS::attach(void(*fn)()){
Tomo073 0:2c0d50143d85 65 serial.attach(fn, Serial::RxIrq);
Tomo073 0:2c0d50143d85 66 }
Tomo073 0:2c0d50143d85 67
Tomo073 0:2c0d50143d85 68 void GPS::GetDefault(){
Tomo073 0:2c0d50143d85 69 datadefault = serial.getc();
Tomo073 0:2c0d50143d85 70 }
Tomo073 0:2c0d50143d85 71
Tomo073 0:2c0d50143d85 72 void GPS::ExtractionData(){
Tomo073 0:2c0d50143d85 73
Tomo073 0:2c0d50143d85 74 Initialization();
Tomo073 0:2c0d50143d85 75
Tomo073 0:2c0d50143d85 76 datadefault = serial.getc();
Tomo073 0:2c0d50143d85 77
Tomo073 0:2c0d50143d85 78 if(datadefault == '$'){
Tomo073 0:2c0d50143d85 79
Tomo073 0:2c0d50143d85 80 char DATA[20] = "\0";
Tomo073 0:2c0d50143d85 81 Initialization();
Tomo073 0:2c0d50143d85 82 getstring(DATA);
Tomo073 0:2c0d50143d85 83
Tomo073 0:2c0d50143d85 84 if(!strcmp(DATA, "GPGGA")){
Tomo073 1:2f131675f872 85 //int info;
Tomo073 1:2f131675f872 86 for(info = 1; info < 6;info++){
Tomo073 0:2c0d50143d85 87 getstring(DATA);
Tomo073 1:2f131675f872 88 switch(info){
Tomo073 0:2c0d50143d85 89 case 1:
Tomo073 0:2c0d50143d85 90 time = strtof(DATA, NULL) + 90000.0;
Tomo073 0:2c0d50143d85 91 break;
Tomo073 0:2c0d50143d85 92 case 2:
Tomo073 0:2c0d50143d85 93 //latitude = strtof(DATA, NULL);
Tomo073 0:2c0d50143d85 94 fMinutes = modff(strtof(DATA, NULL), &latitude)*60.0;
Tomo073 0:2c0d50143d85 95 latitude = latitude*100.0 + fMinutes;
Tomo073 0:2c0d50143d85 96 break;
Tomo073 0:2c0d50143d85 97 case 3:
Tomo073 0:2c0d50143d85 98 if(!strcmp(DATA, "S")){
Tomo073 0:2c0d50143d85 99 latitude = latitude*-1.0;
Tomo073 0:2c0d50143d85 100 }
Tomo073 0:2c0d50143d85 101 break;
Tomo073 0:2c0d50143d85 102 case 4:
Tomo073 0:2c0d50143d85 103 //longtitude = strtof(DATA, NULL);
Tomo073 0:2c0d50143d85 104 fMinutes = modff(strtof(DATA, NULL), &longtitude)*60.0;
Tomo073 0:2c0d50143d85 105 longtitude = longtitude*100.0 + fMinutes;
Tomo073 0:2c0d50143d85 106 break;
Tomo073 0:2c0d50143d85 107 case 5:
Tomo073 0:2c0d50143d85 108 if(!strcmp(DATA, "W")){
Tomo073 0:2c0d50143d85 109 longtitude = longtitude*-1.0;
Tomo073 0:2c0d50143d85 110 }
Tomo073 0:2c0d50143d85 111 break;
Tomo073 0:2c0d50143d85 112 case 9:
Tomo073 0:2c0d50143d85 113 altitude = strtof(DATA, NULL);
Tomo073 0:2c0d50143d85 114 break;
Tomo073 0:2c0d50143d85 115 case 10:
Tomo073 0:2c0d50143d85 116 if(strcmp(DATA, "M")){
Tomo073 0:2c0d50143d85 117 altitude = altitude*-1.0;
Tomo073 0:2c0d50143d85 118 }
Tomo073 0:2c0d50143d85 119 //m = 1;
Tomo073 0:2c0d50143d85 120 readable = true;
Tomo073 0:2c0d50143d85 121 break;
Tomo073 0:2c0d50143d85 122 }
Tomo073 0:2c0d50143d85 123 }
Tomo073 0:2c0d50143d85 124 }
Tomo073 0:2c0d50143d85 125 }
Tomo073 0:2c0d50143d85 126 }
Tomo073 0:2c0d50143d85 127
Tomo073 0:2c0d50143d85 128 void GPS::ExtractionData_FULL(){
Tomo073 0:2c0d50143d85 129
Tomo073 0:2c0d50143d85 130 char DATA[20] = "\0";
Tomo073 0:2c0d50143d85 131
Tomo073 0:2c0d50143d85 132 Initialization();
Tomo073 1:2f131675f872 133
Tomo073 1:2f131675f872 134 if(!reasonable){
Tomo073 0:2c0d50143d85 135 datadefault = serial.getc();
Tomo073 0:2c0d50143d85 136
Tomo073 1:2f131675f872 137 if(datadefault == '$'){
Tomo073 1:2f131675f872 138
Tomo073 1:2f131675f872 139 getstring(DATA);
Tomo073 1:2f131675f872 140
Tomo073 1:2f131675f872 141 if(!strcmp(DATA, "GPGGA")){
Tomo073 1:2f131675f872 142 reasonable = true;
Tomo073 1:2f131675f872 143 }
Tomo073 0:2c0d50143d85 144 }
Tomo073 1:2f131675f872 145
Tomo073 1:2f131675f872 146 }else{
Tomo073 0:2c0d50143d85 147
Tomo073 0:2c0d50143d85 148 Initialization();
Tomo073 0:2c0d50143d85 149 getstring(DATA);
Tomo073 0:2c0d50143d85 150
Tomo073 1:2f131675f872 151 switch(info){
Tomo073 0:2c0d50143d85 152 case 1:
Tomo073 0:2c0d50143d85 153 Seconds = modff((strtof(DATA, NULL) + 90000.0)/100.0, &fHours)*100;
Tomo073 0:2c0d50143d85 154 fMinutes = modff(fHours/100.0, &fHours)*100;
Tomo073 0:2c0d50143d85 155 Hours = fHours;
Tomo073 0:2c0d50143d85 156 Minutes = fMinutes;
Tomo073 0:2c0d50143d85 157 sprintf(timeFULL, "%d:%d:%f", Hours, Minutes, Seconds);
Tomo073 0:2c0d50143d85 158 break;
Tomo073 0:2c0d50143d85 159 case 2:
Tomo073 0:2c0d50143d85 160 fMinutes = modff(strtof(DATA, NULL)/100.0, &fDegrees)*100.0;
Tomo073 0:2c0d50143d85 161 Seconds = modff(strtof(DATA, NULL), &fMinutes)*60.0;
Tomo073 0:2c0d50143d85 162 Degrees = fDegrees;
Tomo073 0:2c0d50143d85 163 Minutes = fMinutes;
Tomo073 0:2c0d50143d85 164 sprintf(latitudeFULL, "%d°%d\'%f\"", Degrees, Minutes, Seconds);
Tomo073 0:2c0d50143d85 165 break;
Tomo073 0:2c0d50143d85 166 case 3:
Tomo073 0:2c0d50143d85 167 sprintf(latitudeFULL, "%s%s", latitudeFULL, DATA);
Tomo073 0:2c0d50143d85 168 break;
Tomo073 0:2c0d50143d85 169 case 4:
Tomo073 0:2c0d50143d85 170 fMinutes = modff(strtof(DATA, NULL)/100.0, &fDegrees)*100.0;
Tomo073 0:2c0d50143d85 171 Seconds = modff(strtof(DATA, NULL), &fMinutes)*60.0;
Tomo073 0:2c0d50143d85 172 Degrees = fDegrees;
Tomo073 0:2c0d50143d85 173 Minutes = fMinutes;
Tomo073 0:2c0d50143d85 174 sprintf(longtitudeFULL, "%d°%d\'%f\"", Degrees, Minutes, Seconds);
Tomo073 0:2c0d50143d85 175 break;
Tomo073 0:2c0d50143d85 176 case 5:
Tomo073 0:2c0d50143d85 177 sprintf(longtitudeFULL, "%s%s", longtitudeFULL, DATA);
Tomo073 0:2c0d50143d85 178 break;
Tomo073 0:2c0d50143d85 179 case 9:
Tomo073 0:2c0d50143d85 180 strcpy(altitudeFULL, DATA);
Tomo073 0:2c0d50143d85 181 break;
Tomo073 0:2c0d50143d85 182 case 10:
Tomo073 0:2c0d50143d85 183 sprintf(altitudeFULL, "%s%s", altitudeFULL, DATA);
Tomo073 1:2f131675f872 184 info = 0;
Tomo073 0:2c0d50143d85 185 reasonable = false;
Tomo073 0:2c0d50143d85 186 readable = true;
Tomo073 0:2c0d50143d85 187 break;
Tomo073 0:2c0d50143d85 188 }
Tomo073 0:2c0d50143d85 189
Tomo073 1:2f131675f872 190 info++;
Tomo073 0:2c0d50143d85 191 //sizeof
Tomo073 0:2c0d50143d85 192 }
Tomo073 0:2c0d50143d85 193 }
Tomo073 0:2c0d50143d85 194
Tomo073 0:2c0d50143d85 195 void GPS::getstring(char a[]){
Tomo073 0:2c0d50143d85 196 int n;
Tomo073 0:2c0d50143d85 197 for(n = 0;;n++){
Tomo073 0:2c0d50143d85 198 char ch = serial.getc();
Tomo073 0:2c0d50143d85 199 if(ch == ',' ||ch == '\n'){
Tomo073 0:2c0d50143d85 200 a[n] = '\0';
Tomo073 0:2c0d50143d85 201 break;
Tomo073 0:2c0d50143d85 202 }
Tomo073 0:2c0d50143d85 203 a[n] = ch;
Tomo073 0:2c0d50143d85 204 }
Tomo073 0:2c0d50143d85 205 }
Tomo073 0:2c0d50143d85 206
Tomo073 0:2c0d50143d85 207 void GPS::Initialization(){
Tomo073 0:2c0d50143d85 208 readable = false;
Tomo073 0:2c0d50143d85 209 Degrees = -1;
Tomo073 0:2c0d50143d85 210 Minutes = -1;
Tomo073 0:2c0d50143d85 211 Hours = -1;
Tomo073 0:2c0d50143d85 212 longtitude = -1.0;
Tomo073 0:2c0d50143d85 213 latitude = -1.0;
Tomo073 0:2c0d50143d85 214 altitude = -1.0;
Tomo073 0:2c0d50143d85 215 time = -1.0;
Tomo073 0:2c0d50143d85 216 fDegrees = -1.0;
Tomo073 0:2c0d50143d85 217 fMinutes = -1.0;
Tomo073 0:2c0d50143d85 218 Seconds = -1.0;
Tomo073 0:2c0d50143d85 219 fHours = -1.0;
Tomo073 0:2c0d50143d85 220 longtitudeFULL = "\0";
Tomo073 0:2c0d50143d85 221 latitudeFULL = "\0";
Tomo073 0:2c0d50143d85 222 timeFULL = "\0";
Tomo073 0:2c0d50143d85 223 altitudeFULL = "\0";
Tomo073 1:2f131675f872 224 }