.
Diff: GPS.cpp
- Revision:
- 8:dfe58484f580
- Child:
- 9:33a8f794633c
diff -r 5c4cbeee68fa -r dfe58484f580 GPS.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPS.cpp Fri Nov 23 21:05:48 2018 +0000 @@ -0,0 +1,126 @@ +#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::Rx_interrupt2() { + while(logGPS.readable() ){ + c = logGPS.getc(); + if(c == '$'){ + rx_in = 0; + ready2analyse = false; + for(uint8_t k=0;k<buffer_size;k++) + buf[k]='-'; + } + else if(rx_in == 0 && c == 'G') + rx_in++; + else if(rx_in == 1 && c == 'N') + rx_in++; + else if(rx_in == 2 && c == 'G') + rx_in++; + else if(rx_in == 3 && c == 'G') + rx_in++; + else if(rx_in == 4 && c == 'A') + rx_in++; + else if(rx_in > 4 && rx_in < buffer_size && is_analysed) + rx_in++; + buf[rx_in] = c; + if(c== '*'){ + str = string(buf); + str = str.substr(0,rx_in); + ready2analyse = true; + is_analysed = false; + } + } + return; +}*/ +void GPS::return_string(string *st){ + *st = string(buf); + return; + + } \ No newline at end of file