GPS.cpp

00001 #include "GPS.h"
00002 #include "math.h"
00003 #include "string"
00004 #include "mbed.h"
00005 
00006 using namespace std;
00007 
00008 GPS::GPS(PinName P1,PinName P2,float TS) : logGPS(P1, P2),thread(osPriorityLow, 4096) {  
00009     
00010     // start thread and timer interrupt
00011     logGPS.baud(9600);
00012     logGPS.attach(callback(this, &GPS::Rx_interrupt), Serial::RxIrq);
00013     thread.start(callback(this, &GPS::get_data));
00014     ticker.attach(callback(this, &GPS::sendSignal), TS);
00015     buffer_filled = false;
00016     rx_in = 0;
00017 }
00018 
00019 GPS::~GPS() {}
00020 
00021 void GPS::get_data(void){
00022     float dum;
00023     string str2;
00024     while(1) {
00025         thread.signal_wait(signal);
00026         //string str="blabla $GNGGA,124720.00,4729.84871,N,00843.83588,E,1,06,2.77,406.1,M,47.4,M,,*4B";
00027         str=string(buf);
00028         size_t found=str.find("GNGGA");
00029         if(found!=string::npos){
00030             int cou=1;
00031             str.replace(0,found+6, "\0");
00032             do{
00033                 int en=str.find(",");
00034                 str2=str.substr(0,en);
00035                 switch(cou){
00036                     case 2:{
00037                         float north = atof(str2.c_str());
00038                         dum=floor(north/100.0);
00039                         north=dum+((north/100.0)-dum)/0.6;
00040                         pos0_xyz[0]=north;
00041                         break;
00042                         }
00043                     case 4:{
00044                         float ew = atof(str2.c_str());
00045                         dum=floor(ew/100.0);
00046                         ew=dum+((ew/100.0)-dum)/0.6;
00047                         pos0_xyz[1]=ew;
00048                         break;
00049                         }
00050                     case 6:{
00051                         int isvalid = atof(str2.c_str());
00052                         break;
00053                         }
00054                     case 7:{
00055                         int numSat = atof(str2.c_str());
00056                         break;
00057                         }
00058                     case 9:{
00059                         float alt = atof(str2.c_str());
00060                         pos0_xyz[2]=alt;
00061                         break;
00062                         }
00063                     }
00064                 str.replace(0,en+1,"\0");
00065                 cou++;
00066                 }while(cou<10 && str.length()>2);
00067              }// if found
00068         }//while(1)       
00069     }
00070 void GPS::sendSignal() {
00071     
00072     thread.signal_set(signal);
00073 }
00074 
00075 void GPS::get_position(void){
00076     pos_xyz[0]=pos0_xyz[0];
00077     pos_xyz[1]=pos0_xyz[1];
00078     pos_xyz[2]=pos0_xyz[2];
00079     }
00080 
00081 
00082 
00083 void GPS::Rx_interrupt() {
00084     while ((logGPS.readable()) ){
00085         buf[rx_in++] = logGPS.getc(); 
00086         if(rx_in==255)
00087             buffer_filled = true;
00088     }
00089 }
00090 
00091 void GPS::return_string(string *st){
00092     *st = string(buf);
00093     return;    
00094     
00095     }