Diff: GPS.cpp

Revision:

2:4ebd0e486b5a

Parent:

1:455c3e604c92

--- a/GPS.cpp	Sat Mar 30 08:11:26 2013 +0000
+++ b/GPS.cpp	Mon Aug 19 08:51:14 2013 +0000
@@ -1,98 +1,85 @@
 #include "GPS.h"
 
+#define pi 3.14159265359
+#define r 6367.444657
+
 GPS::GPS(PinName tx,PinName rx):_gps(tx,rx)
 {
  _gps.baud(4800);
- longitude=0.0;
- latitude=0.0;
 }
 
 int GPS::sample()
 {
- float dummy,d1,d2;
- int lock;
+ float d1,d2;
+  
  while(1) 
  {
   getline();
-  fok_jel='°';
-  if(sscanf(msg,"GPGGA,%f,%f,%c,%f,%c,%d,%f,%f,%f",&time,&latitude,&ns,&longitude,&ew,&lock,&d1,&d2,&altitude)>=1) 
+  if(sscanf(msg,"GPGGA,%2d%2d%2d,%f,%c,%f,%c,%c,%f,%f,%f",
+                &poz.ora,&poz.perc,&poz.masodperc,
+                &poz.szel,&poz.szb,
+                &poz.hossz,&poz.hb,
+                &poz.flag,
+                &d1,&d2,
+                &poz.magassag)>=1) 
   {
-   ora=elso2(time)+1;
-   perc=kozepso2(time);
-   masodperc=utolso2(time);
-   if(lock==0) 
-   {
-    nincs_adat();
-    altitude=0.0;
-   } 
-   fokpercmasodperc();
+   printf("%f %f\n\r",poz.szel,poz.hossz);
+   //delta_t=(clock()-start)/CLOCKS_PER_SEC;
+   //start=clock();
+   //if (delta_t==0) delta_t=1.0;
+   //if(poz.flag=='1') nincs_adat();
+   //tav=tavolsag(poz.szel,poz.hossz,old_sz,old_h);
+   //vit=tav/delta_t*3.6;
+   //poz_old=poz_new;
    return 1;
   }
-  if(sscanf(msg,"GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,%f",&time,&status,&latitude,&ns,&longitude,&ew,&speed,&dummy,&date)>=1) 
+  /*if(sscanf(msg,"GPRMC,%2d%2d%2d,%c,%f,%c,%f,%c,%f,%f,%2d%2d%2d",
+                &poz.ora,&poz.perc,&poz.masodperc,
+                &poz.flag,
+                &poz.szel,&poz.szb,
+                &poz.hossz,&poz.hb,
+                &poz.sebesseg,
+                &d1,
+                &poz.nap,&poz.honap,&poz.ev)>=1) 
   {
-   nap=elso2(date);
-   ho=kozepso2(date);
-   ev=utolso2(date);
-   ora=elso2(time)+1;
-   perc=kozepso2(time);
-   masodperc=utolso2(time);
-   if(status=='V') 
-   {
-    nincs_adat();
-    speed=0.0;
-   }
-   fokpercmasodperc();
+   delta_t=(clock()-start)/CLOCKS_PER_SEC;
+   start=clock();
+   if (delta_t==0) delta_t=1.0;
+   if(poz.flag=='V') nincs_adat();
+   //tav=tavolsag(poz_new.szel,poz_new.hossz,poz_old.szel,poz_old.hossz);
+   //vit=tav/delta_t*3.6;
+   //poz_old=poz_new;
    return 1;
-  }
+  }*/
  }
 }
 
-void GPS::fokpercmasodperc()
-{
- long_fok=fok_szamol(longitude);
- long_perc=perc_szamol(longitude);
- long_masodperc=masodperc_szamol(longitude);
- lati_fok=fok_szamol(latitude);
- lati_perc=perc_szamol(latitude);
- lati_masodperc=masodperc_szamol(latitude);
-}
-
 void GPS::nincs_adat()
 {
- longitude=0.0;
- ew='?';
- latitude=0.0;
- ns='?';
-}
-
-int GPS::fok_szamol(float f)
-{
- return (int)(f/100);
-}
-
-int GPS::perc_szamol(float f)
-{
- return (int)(f-fok_szamol(f)*100);
+ poz.hossz=0.0;
+ poz.hb='?';
+ poz.szel=0.0;
+ poz.szb='?';
+ poz.sebesseg=0.0;
+ poz.magassag=0.0;
 }
 
-float GPS::masodperc_szamol(float f)
-{
- return (f-fok_szamol(f)*100-perc_szamol(f))*60;
-}
-
-int GPS::elso2(float d)
+float GPS::tavolsag(float szel1,float hossz1,float szel2,float hossz2)
 {
- return (int)floor(d/10000);
-}
+ float x1,y1,z1,x2,y2,z2;
+ float h1,h2,sz1,sz2;
 
-int GPS::kozepso2(float d)
-{
- return (int)floor((d-elso2(d)*10000)/100);
-}
-
-int GPS::utolso2(float d)
-{
- return (int)floor(d-elso2(d)*10000-kozepso2(d)*100);
+ sz1=(int)(szel1/100)+(szel1-100*(int)(szel1/100))/60;
+ h1=(int)(hossz1/100)+(hossz1-100*(int)(hossz1/100))/60;
+ sz2=(int)(szel2/100)+(szel2-100*(int)(szel2/100))/60;
+ h2=(int)(hossz2/100)+(hossz2-100*(int)(hossz2/100))/60;
+ x1=r*cos(sz1)*sin(h1);
+ y1=r*cos(sz1)*cos(h1);
+ z1=r*sin(sz1);
+ x2=r*cos(sz2)*sin(h2);
+ y2=r*cos(sz2)*cos(h2);
+ z2=r*sin(sz2);
+ return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1))*1000;
 }
 
 void GPS::getline()