Ekyu Kondo / GPS_Interface

Mbed library to get location informatiuon from GYSFDMAXB

Dependents:   Izu_2019

GPS.cpp@5:6231c440275d, 2019-03-02 (annotated)

Committer:
zeutel
Date:
Sat Mar 02 15:15:29 2019 +0000
Revision:
5:6231c440275d
Parent:
1:68601d2af8a5 
GPS

Who changed what in which revision?

UserRevisionLine numberNew 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;
zeutel 5:6231c440275d 7 longitude=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
zeutel 5:6231c440275d 29 //_gps.attach(this,&GPS::getgps);
zeutel 5:6231c440275d 30 }
zeutel 5:6231c440275d 31
zeutel 5:6231c440275d 32 GPS::~GPS()
zeutel 5:6231c440275d 33 {
kim1212 0:4cc88ba7719b 34 }
kim1212 0:4cc88ba7719b 35
kim1212 0:4cc88ba7719b 36 void GPS::gpscmd(char *command)
kim1212 0:4cc88ba7719b 37 {
kim1212 0:4cc88ba7719b 38 int len=strlen(command);
kim1212 0:4cc88ba7719b 39 char xordata[len+4];
kim1212 0:4cc88ba7719b 40 sprintf(xordata,"PMTK%s",command);
kim1212 0:4cc88ba7719b 41
kim1212 0:4cc88ba7719b 42
kim1212 0:4cc88ba7719b 43 char xorbuf=0x00;
kim1212 0:4cc88ba7719b 44 for(int i=0;i<len+4;i++){
kim1212 0:4cc88ba7719b 45 xorbuf=xorbuf^xordata[i];
kim1212 0:4cc88ba7719b 46 }
kim1212 0:4cc88ba7719b 47
kim1212 0:4cc88ba7719b 48
kim1212 0:4cc88ba7719b 49 char packet[len+8];
kim1212 0:4cc88ba7719b 50 if(xorbuf>0x0f){
kim1212 0:4cc88ba7719b 51 sprintf(packet,"$%s*%x",xordata,xorbuf);
kim1212 0:4cc88ba7719b 52 }
kim1212 0:4cc88ba7719b 53 else {
kim1212 0:4cc88ba7719b 54 sprintf(packet,"$%s*0%x",xordata,xorbuf);
kim1212 0:4cc88ba7719b 55 }
kim1212 0:4cc88ba7719b 56
kim1212 0:4cc88ba7719b 57
kim1212 0:4cc88ba7719b 58 _gps.printf(packet);
kim1212 0:4cc88ba7719b 59 _gps.putc(0x0D);
kim1212 0:4cc88ba7719b 60 _gps.putc(0x0A);
kim1212 0:4cc88ba7719b 61 }
kim1212 0:4cc88ba7719b 62
kim1212 0:4cc88ba7719b 63 void GPS::getgps()
kim1212 0:4cc88ba7719b 64 {
kim1212 0:4cc88ba7719b 65 if(_gps.getc()=='$'){
kim1212 0:4cc88ba7719b 66
kim1212 0:4cc88ba7719b 67 int counter;
kim1212 0:4cc88ba7719b 68 char sample=0x00;
kim1212 0:4cc88ba7719b 69
kim1212 0:4cc88ba7719b 70 int point1=0;
kim1212 0:4cc88ba7719b 71 int point2=0;
kim1212 0:4cc88ba7719b 72
kim1212 0:4cc88ba7719b 73
kim1212 0:4cc88ba7719b 74 for(counter=0;counter<18;counter++){
kim1212 0:4cc88ba7719b 75 sample=_gps.getc();
kim1212 0:4cc88ba7719b 76 }
kim1212 0:4cc88ba7719b 77
kim1212 0:4cc88ba7719b 78 if(sample=='A'){
kim1212 0:4cc88ba7719b 79
kim1212 0:4cc88ba7719b 80 //初期化
kim1212 0:4cc88ba7719b 81 latitude=0;
zeutel 5:6231c440275d 82 longitude=0;
kim1212 0:4cc88ba7719b 83
kim1212 0:4cc88ba7719b 84 //データ取得
kim1212 0:4cc88ba7719b 85 for(counter=0;sample!='\n';counter++){
kim1212 0:4cc88ba7719b 86 sample=_gps.getc();
kim1212 0:4cc88ba7719b 87 buf[counter]=sample;
kim1212 0:4cc88ba7719b 88
kim1212 0:4cc88ba7719b 89 if(buf[counter]=='.'&&point1==0){
kim1212 0:4cc88ba7719b 90 point1=counter;
kim1212 0:4cc88ba7719b 91 }
kim1212 0:4cc88ba7719b 92
kim1212 0:4cc88ba7719b 93 if(counter>8&&buf[counter]=='.'&&point2==0){
kim1212 0:4cc88ba7719b 94 point2=counter;
kim1212 0:4cc88ba7719b 95 }
kim1212 0:4cc88ba7719b 96 }
kim1212 0:4cc88ba7719b 97
kim1212 0:4cc88ba7719b 98 //緯度計算
kim1212 0:4cc88ba7719b 99 for(counter=1;counter<point1-2;counter++){
kim1212 0:4cc88ba7719b 100
kim1212 0:4cc88ba7719b 101 float num=1;
kim1212 0:4cc88ba7719b 102 for(int i=point1-counter-3;i>0;i--){
kim1212 0:4cc88ba7719b 103 num=num*10;
kim1212 0:4cc88ba7719b 104 }
kim1212 0:4cc88ba7719b 105
kim1212 0:4cc88ba7719b 106 latitude+=(float)num*(buf[counter]-48);
kim1212 0:4cc88ba7719b 107 }
kim1212 0:4cc88ba7719b 108
kim1212 0:4cc88ba7719b 109 float minute=0;
kim1212 0:4cc88ba7719b 110
kim1212 0:4cc88ba7719b 111 minute+=(buf[point1-2]-48)*10;
kim1212 0:4cc88ba7719b 112 minute+=(buf[point1-1]-48);
kim1212 0:4cc88ba7719b 113
kim1212 0:4cc88ba7719b 114 for(counter=point1+1;counter<point1+5;counter++){
kim1212 0:4cc88ba7719b 115 float num=1;
kim1212 0:4cc88ba7719b 116
kim1212 0:4cc88ba7719b 117 for(int i=counter-point1;i>0;i--){
kim1212 1:68601d2af8a5 118 num=num*0.1f;
kim1212 0:4cc88ba7719b 119 }
kim1212 0:4cc88ba7719b 120
kim1212 0:4cc88ba7719b 121 minute+=(float)num*(buf[counter]-48);
kim1212 0:4cc88ba7719b 122 }
kim1212 0:4cc88ba7719b 123
kim1212 0:4cc88ba7719b 124 latitude+=minute/60;
kim1212 0:4cc88ba7719b 125
kim1212 0:4cc88ba7719b 126
kim1212 0:4cc88ba7719b 127 //経度計算
kim1212 0:4cc88ba7719b 128 for(counter=point1+8;counter<point2-2;counter++){
kim1212 0:4cc88ba7719b 129
kim1212 0:4cc88ba7719b 130 float num=1;
kim1212 0:4cc88ba7719b 131 for(int i=point2-counter-3;i>0;i--){
kim1212 0:4cc88ba7719b 132 num=num*10;
kim1212 0:4cc88ba7719b 133 }
kim1212 0:4cc88ba7719b 134
zeutel 5:6231c440275d 135 longitude+=(float)num*(buf[counter]-48);
kim1212 0:4cc88ba7719b 136 }
kim1212 0:4cc88ba7719b 137
kim1212 0:4cc88ba7719b 138 minute=0;
kim1212 0:4cc88ba7719b 139
kim1212 0:4cc88ba7719b 140 minute+=(buf[point2-2]-48)*10;
kim1212 0:4cc88ba7719b 141 minute+=(buf[point2-1]-48);
kim1212 0:4cc88ba7719b 142
kim1212 0:4cc88ba7719b 143 for(counter=point2+1;counter<point2+5;counter++){
kim1212 0:4cc88ba7719b 144 float num=1;
kim1212 0:4cc88ba7719b 145
kim1212 0:4cc88ba7719b 146 for(int i=counter-point2;i>0;i--){
kim1212 1:68601d2af8a5 147 num=num*0.1f;
kim1212 0:4cc88ba7719b 148 }
kim1212 0:4cc88ba7719b 149
kim1212 0:4cc88ba7719b 150 minute+=(float)num*(buf[counter]-48);
kim1212 0:4cc88ba7719b 151 }
kim1212 0:4cc88ba7719b 152
zeutel 5:6231c440275d 153 longitude+=minute/60;
kim1212 0:4cc88ba7719b 154
kim1212 0:4cc88ba7719b 155
kim1212 0:4cc88ba7719b 156 result=true;
kim1212 0:4cc88ba7719b 157 }
kim1212 0:4cc88ba7719b 158
kim1212 0:4cc88ba7719b 159 else{
kim1212 0:4cc88ba7719b 160 result=false;
kim1212 0:4cc88ba7719b 161 }
kim1212 0:4cc88ba7719b 162
kim1212 0:4cc88ba7719b 163
kim1212 0:4cc88ba7719b 164
kim1212 0:4cc88ba7719b 165
kim1212 0:4cc88ba7719b 166
kim1212 0:4cc88ba7719b 167 }
kim1212 0:4cc88ba7719b 168 }