.
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 13 2022 23:05:32 by 1.7.2