GPS.cpp

00001 #include "GPS.h"
00002 
00003 #define pi 3.14159265359
00004 #define r 6367.444657
00005 
00006 GPS::GPS(PinName tx,PinName rx):_gps(tx,rx)
00007 {
00008  _gps.baud(4800);
00009 }
00010 
00011 int GPS::sample()
00012 {
00013  float d1,d2;
00014   
00015  while(1) 
00016  {
00017   getline();
00018   if(sscanf(msg,"GPGGA,%2d%2d%2d,%f,%c,%f,%c,%c,%f,%f,%f",
00019                 &poz.ora,&poz.perc,&poz.masodperc,
00020                 &poz.szel,&poz.szb,
00021                 &poz.hossz,&poz.hb,
00022                 &poz.flag,
00023                 &d1,&d2,
00024                 &poz.magassag)>=1) 
00025   {
00026    printf("%f %f\n\r",poz.szel,poz.hossz);
00027    //delta_t=(clock()-start)/CLOCKS_PER_SEC;
00028    //start=clock();
00029    //if (delta_t==0) delta_t=1.0;
00030    //if(poz.flag=='1') nincs_adat();
00031    //tav=tavolsag(poz.szel,poz.hossz,old_sz,old_h);
00032    //vit=tav/delta_t*3.6;
00033    //poz_old=poz_new;
00034    return 1;
00035   }
00036   /*if(sscanf(msg,"GPRMC,%2d%2d%2d,%c,%f,%c,%f,%c,%f,%f,%2d%2d%2d",
00037                 &poz.ora,&poz.perc,&poz.masodperc,
00038                 &poz.flag,
00039                 &poz.szel,&poz.szb,
00040                 &poz.hossz,&poz.hb,
00041                 &poz.sebesseg,
00042                 &d1,
00043                 &poz.nap,&poz.honap,&poz.ev)>=1) 
00044   {
00045    delta_t=(clock()-start)/CLOCKS_PER_SEC;
00046    start=clock();
00047    if (delta_t==0) delta_t=1.0;
00048    if(poz.flag=='V') nincs_adat();
00049    //tav=tavolsag(poz_new.szel,poz_new.hossz,poz_old.szel,poz_old.hossz);
00050    //vit=tav/delta_t*3.6;
00051    //poz_old=poz_new;
00052    return 1;
00053   }*/
00054  }
00055 }
00056 
00057 void GPS::nincs_adat()
00058 {
00059  poz.hossz=0.0;
00060  poz.hb='?';
00061  poz.szel=0.0;
00062  poz.szb='?';
00063  poz.sebesseg=0.0;
00064  poz.magassag=0.0;
00065 }
00066 
00067 float GPS::tavolsag(float szel1,float hossz1,float szel2,float hossz2)
00068 {
00069  float x1,y1,z1,x2,y2,z2;
00070  float h1,h2,sz1,sz2;
00071 
00072  sz1=(int)(szel1/100)+(szel1-100*(int)(szel1/100))/60;
00073  h1=(int)(hossz1/100)+(hossz1-100*(int)(hossz1/100))/60;
00074  sz2=(int)(szel2/100)+(szel2-100*(int)(szel2/100))/60;
00075  h2=(int)(hossz2/100)+(hossz2-100*(int)(hossz2/100))/60;
00076  x1=r*cos(sz1)*sin(h1);
00077  y1=r*cos(sz1)*cos(h1);
00078  z1=r*sin(sz1);
00079  x2=r*cos(sz2)*sin(h2);
00080  y2=r*cos(sz2)*cos(h2);
00081  z2=r*sin(sz2);
00082  return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1))*1000;
00083 }
00084 
00085 void GPS::getline()
00086 {
00087  while(_gps.getc() != '$');    // wait for the start of a line
00088  for(int i=0;i<256;i++) 
00089  {
00090   msg[i]=_gps.getc();
00091   if(msg[i]=='\r') 
00092   {
00093    msg[i]=0;
00094    return;
00095   }
00096  }
00097  error("Tul hosszu szoveg !");
00098 }