Diff: GPS.cpp

Revision:

0:3312872854c4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,95 @@
+#include "GPS.h"
+#include "math.h"
+#include "string"
+#include "mbed.h"
+
+using namespace std;
+
+GPS::GPS(PinName P1,PinName P2,float TS) : logGPS(P1, P2),thread(osPriorityLow, 4096) {  
+    
+    // start thread and timer interrupt
+    logGPS.baud(9600);
+    logGPS.attach(callback(this, &GPS::Rx_interrupt), Serial::RxIrq);
+    thread.start(callback(this, &GPS::get_data));
+    ticker.attach(callback(this, &GPS::sendSignal), TS);
+    buffer_filled = false;
+    rx_in = 0;
+}
+
+GPS::~GPS() {}
+
+void GPS::get_data(void){
+    float dum;
+    string str2;
+    while(1) {
+        thread.signal_wait(signal);
+        //string str="blabla $GNGGA,124720.00,4729.84871,N,00843.83588,E,1,06,2.77,406.1,M,47.4,M,,*4B";
+        str=string(buf);
+        size_t found=str.find("GNGGA");
+        if(found!=string::npos){
+            int cou=1;
+            str.replace(0,found+6, "\0");
+            do{
+                int en=str.find(",");
+                str2=str.substr(0,en);
+                switch(cou){
+                    case 2:{
+                        float north = atof(str2.c_str());
+                        dum=floor(north/100.0);
+                        north=dum+((north/100.0)-dum)/0.6;
+                        pos0_xyz[0]=north;
+                        break;
+                        }
+                    case 4:{
+                        float ew = atof(str2.c_str());
+                        dum=floor(ew/100.0);
+                        ew=dum+((ew/100.0)-dum)/0.6;
+                        pos0_xyz[1]=ew;
+                        break;
+                        }
+                    case 6:{
+                        int isvalid = atof(str2.c_str());
+                        break;
+                        }
+                    case 7:{
+                        int numSat = atof(str2.c_str());
+                        break;
+                        }
+                    case 9:{
+                        float alt = atof(str2.c_str());
+                        pos0_xyz[2]=alt;
+                        break;
+                        }
+                    }
+                str.replace(0,en+1,"\0");
+                cou++;
+                }while(cou<10 && str.length()>2);
+             }// if found
+        }//while(1)       
+    }
+void GPS::sendSignal() {
+    
+    thread.signal_set(signal);
+}
+
+void GPS::get_position(void){
+    pos_xyz[0]=pos0_xyz[0];
+    pos_xyz[1]=pos0_xyz[1];
+    pos_xyz[2]=pos0_xyz[2];
+    }
+
+
+
+void GPS::Rx_interrupt() {
+    while ((logGPS.readable()) ){
+        buf[rx_in++] = logGPS.getc(); 
+        if(rx_in==255)
+            buffer_filled = true;
+    }
+}
+
+void GPS::return_string(string *st){
+    *st = string(buf);
+    return;    
+    
+    }
\ No newline at end of file