Levente Szabó / GPS

Library for the EM-406 GPS module

Fork of GPS by Simon Ford

GPS.cpp@2:4ebd0e486b5a, 2013-08-19 (annotated)

Committer:
szabolev
Date:
Mon Aug 19 08:51:14 2013 +0000
Revision:
2:4ebd0e486b5a
Parent:
1:455c3e604c92 
init

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simon 0:15611c7938a3 1 #include "GPS.h"
simon 0:15611c7938a3 2
szabolev 2:4ebd0e486b5a 3 #define pi 3.14159265359
szabolev 2:4ebd0e486b5a 4 #define r 6367.444657
szabolev 2:4ebd0e486b5a 5
szabolev 1:455c3e604c92 6 GPS::GPS(PinName tx,PinName rx):_gps(tx,rx)
szabolev 1:455c3e604c92 7 {
szabolev 1:455c3e604c92 8 _gps.baud(4800);
szabolev 1:455c3e604c92 9 }
szabolev 1:455c3e604c92 10
szabolev 1:455c3e604c92 11 int GPS::sample()
szabolev 1:455c3e604c92 12 {
szabolev 2:4ebd0e486b5a 13 float d1,d2;
szabolev 2:4ebd0e486b5a 14
szabolev 1:455c3e604c92 15 while(1)
szabolev 1:455c3e604c92 16 {
szabolev 1:455c3e604c92 17 getline();
szabolev 2:4ebd0e486b5a 18 if(sscanf(msg,"GPGGA,%2d%2d%2d,%f,%c,%f,%c,%c,%f,%f,%f",
szabolev 2:4ebd0e486b5a 19 &poz.ora,&poz.perc,&poz.masodperc,
szabolev 2:4ebd0e486b5a 20 &poz.szel,&poz.szb,
szabolev 2:4ebd0e486b5a 21 &poz.hossz,&poz.hb,
szabolev 2:4ebd0e486b5a 22 &poz.flag,
szabolev 2:4ebd0e486b5a 23 &d1,&d2,
szabolev 2:4ebd0e486b5a 24 &poz.magassag)>=1)
szabolev 1:455c3e604c92 25 {
szabolev 2:4ebd0e486b5a 26 printf("%f %f\n\r",poz.szel,poz.hossz);
szabolev 2:4ebd0e486b5a 27 //delta_t=(clock()-start)/CLOCKS_PER_SEC;
szabolev 2:4ebd0e486b5a 28 //start=clock();
szabolev 2:4ebd0e486b5a 29 //if (delta_t==0) delta_t=1.0;
szabolev 2:4ebd0e486b5a 30 //if(poz.flag=='1') nincs_adat();
szabolev 2:4ebd0e486b5a 31 //tav=tavolsag(poz.szel,poz.hossz,old_sz,old_h);
szabolev 2:4ebd0e486b5a 32 //vit=tav/delta_t*3.6;
szabolev 2:4ebd0e486b5a 33 //poz_old=poz_new;
szabolev 1:455c3e604c92 34 return 1;
szabolev 1:455c3e604c92 35 }
szabolev 2:4ebd0e486b5a 36 /*if(sscanf(msg,"GPRMC,%2d%2d%2d,%c,%f,%c,%f,%c,%f,%f,%2d%2d%2d",
szabolev 2:4ebd0e486b5a 37 &poz.ora,&poz.perc,&poz.masodperc,
szabolev 2:4ebd0e486b5a 38 &poz.flag,
szabolev 2:4ebd0e486b5a 39 &poz.szel,&poz.szb,
szabolev 2:4ebd0e486b5a 40 &poz.hossz,&poz.hb,
szabolev 2:4ebd0e486b5a 41 &poz.sebesseg,
szabolev 2:4ebd0e486b5a 42 &d1,
szabolev 2:4ebd0e486b5a 43 &poz.nap,&poz.honap,&poz.ev)>=1)
szabolev 1:455c3e604c92 44 {
szabolev 2:4ebd0e486b5a 45 delta_t=(clock()-start)/CLOCKS_PER_SEC;
szabolev 2:4ebd0e486b5a 46 start=clock();
szabolev 2:4ebd0e486b5a 47 if (delta_t==0) delta_t=1.0;
szabolev 2:4ebd0e486b5a 48 if(poz.flag=='V') nincs_adat();
szabolev 2:4ebd0e486b5a 49 //tav=tavolsag(poz_new.szel,poz_new.hossz,poz_old.szel,poz_old.hossz);
szabolev 2:4ebd0e486b5a 50 //vit=tav/delta_t*3.6;
szabolev 2:4ebd0e486b5a 51 //poz_old=poz_new;
szabolev 1:455c3e604c92 52 return 1;
szabolev 2:4ebd0e486b5a 53 }*/
szabolev 1:455c3e604c92 54 }
simon 0:15611c7938a3 55 }
simon 0:15611c7938a3 56
szabolev 1:455c3e604c92 57 void GPS::nincs_adat()
szabolev 1:455c3e604c92 58 {
szabolev 2:4ebd0e486b5a 59 poz.hossz=0.0;
szabolev 2:4ebd0e486b5a 60 poz.hb='?';
szabolev 2:4ebd0e486b5a 61 poz.szel=0.0;
szabolev 2:4ebd0e486b5a 62 poz.szb='?';
szabolev 2:4ebd0e486b5a 63 poz.sebesseg=0.0;
szabolev 2:4ebd0e486b5a 64 poz.magassag=0.0;
simon 0:15611c7938a3 65 }
simon 0:15611c7938a3 66
szabolev 2:4ebd0e486b5a 67 float GPS::tavolsag(float szel1,float hossz1,float szel2,float hossz2)
szabolev 1:455c3e604c92 68 {
szabolev 2:4ebd0e486b5a 69 float x1,y1,z1,x2,y2,z2;
szabolev 2:4ebd0e486b5a 70 float h1,h2,sz1,sz2;
szabolev 1:455c3e604c92 71
szabolev 2:4ebd0e486b5a 72 sz1=(int)(szel1/100)+(szel1-100*(int)(szel1/100))/60;
szabolev 2:4ebd0e486b5a 73 h1=(int)(hossz1/100)+(hossz1-100*(int)(hossz1/100))/60;
szabolev 2:4ebd0e486b5a 74 sz2=(int)(szel2/100)+(szel2-100*(int)(szel2/100))/60;
szabolev 2:4ebd0e486b5a 75 h2=(int)(hossz2/100)+(hossz2-100*(int)(hossz2/100))/60;
szabolev 2:4ebd0e486b5a 76 x1=r*cos(sz1)*sin(h1);
szabolev 2:4ebd0e486b5a 77 y1=r*cos(sz1)*cos(h1);
szabolev 2:4ebd0e486b5a 78 z1=r*sin(sz1);
szabolev 2:4ebd0e486b5a 79 x2=r*cos(sz2)*sin(h2);
szabolev 2:4ebd0e486b5a 80 y2=r*cos(sz2)*cos(h2);
szabolev 2:4ebd0e486b5a 81 z2=r*sin(sz2);
szabolev 2:4ebd0e486b5a 82 return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1))*1000;
simon 0:15611c7938a3 83 }
szabolev 1:455c3e604c92 84
szabolev 1:455c3e604c92 85 void GPS::getline()
szabolev 1:455c3e604c92 86 {
szabolev 1:455c3e604c92 87 while(_gps.getc() != '$'); // wait for the start of a line
szabolev 1:455c3e604c92 88 for(int i=0;i<256;i++)
szabolev 1:455c3e604c92 89 {
szabolev 1:455c3e604c92 90 msg[i]=_gps.getc();
szabolev 1:455c3e604c92 91 if(msg[i]=='\r')
szabolev 1:455c3e604c92 92 {
szabolev 1:455c3e604c92 93 msg[i]=0;
szabolev 1:455c3e604c92 94 return;
szabolev 1:455c3e604c92 95 }
szabolev 1:455c3e604c92 96 }
szabolev 1:455c3e604c92 97 error("Tul hosszu szoveg !");
szabolev 1:455c3e604c92 98 }