Ruprecht Altenburger / Misc_Lib

.

Dependents:  

GPS.cpp@8:dfe58484f580, 2018-11-23 (annotated)

Committer:
altb
Date:
Fri Nov 23 21:05:48 2018 +0000
Revision:
8:dfe58484f580
Child:
9:33a8f794633c
Added GPS read;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 8:dfe58484f580 1 #include "GPS.h"
altb 8:dfe58484f580 2 #include "math.h"
altb 8:dfe58484f580 3 #include "string"
altb 8:dfe58484f580 4 #include "mbed.h"
altb 8:dfe58484f580 5
altb 8:dfe58484f580 6 using namespace std;
altb 8:dfe58484f580 7
altb 8:dfe58484f580 8 GPS::GPS(PinName P1,PinName P2,float TS) : logGPS(P1, P2),thread(osPriorityLow, 4096) {
altb 8:dfe58484f580 9
altb 8:dfe58484f580 10 // start thread and timer interrupt
altb 8:dfe58484f580 11 logGPS.baud(9600);
altb 8:dfe58484f580 12 logGPS.attach(callback(this, &GPS::Rx_interrupt), Serial::RxIrq);
altb 8:dfe58484f580 13 thread.start(callback(this, &GPS::get_data));
altb 8:dfe58484f580 14 ticker.attach(callback(this, &GPS::sendSignal), TS);
altb 8:dfe58484f580 15 buffer_filled = false;
altb 8:dfe58484f580 16 rx_in = 0;
altb 8:dfe58484f580 17 }
altb 8:dfe58484f580 18
altb 8:dfe58484f580 19 GPS::~GPS() {}
altb 8:dfe58484f580 20
altb 8:dfe58484f580 21 void GPS::get_data(void){
altb 8:dfe58484f580 22 float dum;
altb 8:dfe58484f580 23 string str2;
altb 8:dfe58484f580 24 while(1) {
altb 8:dfe58484f580 25 thread.signal_wait(signal);
altb 8:dfe58484f580 26 //string str="blabla $GNGGA,124720.00,4729.84871,N,00843.83588,E,1,06,2.77,406.1,M,47.4,M,,*4B";
altb 8:dfe58484f580 27 str=string(buf);
altb 8:dfe58484f580 28 size_t found=str.find("GNGGA");
altb 8:dfe58484f580 29 if(found!=string::npos){
altb 8:dfe58484f580 30 int cou=1;
altb 8:dfe58484f580 31 str.replace(0,found+6, "\0");
altb 8:dfe58484f580 32 do{
altb 8:dfe58484f580 33 int en=str.find(",");
altb 8:dfe58484f580 34 str2=str.substr(0,en);
altb 8:dfe58484f580 35 switch(cou){
altb 8:dfe58484f580 36 case 2:{
altb 8:dfe58484f580 37 float north = atof(str2.c_str());
altb 8:dfe58484f580 38 dum=floor(north/100.0);
altb 8:dfe58484f580 39 north=dum+((north/100.0)-dum)/0.6;
altb 8:dfe58484f580 40 pos0_xyz[0]=north;
altb 8:dfe58484f580 41 break;
altb 8:dfe58484f580 42 }
altb 8:dfe58484f580 43 case 4:{
altb 8:dfe58484f580 44 float ew = atof(str2.c_str());
altb 8:dfe58484f580 45 dum=floor(ew/100.0);
altb 8:dfe58484f580 46 ew=dum+((ew/100.0)-dum)/0.6;
altb 8:dfe58484f580 47 pos0_xyz[1]=ew;
altb 8:dfe58484f580 48 break;
altb 8:dfe58484f580 49 }
altb 8:dfe58484f580 50 case 6:{
altb 8:dfe58484f580 51 int isvalid = atof(str2.c_str());
altb 8:dfe58484f580 52 break;
altb 8:dfe58484f580 53 }
altb 8:dfe58484f580 54 case 7:{
altb 8:dfe58484f580 55 int numSat = atof(str2.c_str());
altb 8:dfe58484f580 56 break;
altb 8:dfe58484f580 57 }
altb 8:dfe58484f580 58 case 9:{
altb 8:dfe58484f580 59 float alt = atof(str2.c_str());
altb 8:dfe58484f580 60 pos0_xyz[2]=alt;
altb 8:dfe58484f580 61 break;
altb 8:dfe58484f580 62 }
altb 8:dfe58484f580 63 }
altb 8:dfe58484f580 64 str.replace(0,en+1,"\0");
altb 8:dfe58484f580 65 cou++;
altb 8:dfe58484f580 66 }while(cou<10 && str.length()>2);
altb 8:dfe58484f580 67 }// if found
altb 8:dfe58484f580 68 }//while(1)
altb 8:dfe58484f580 69 }
altb 8:dfe58484f580 70 void GPS::sendSignal() {
altb 8:dfe58484f580 71
altb 8:dfe58484f580 72 thread.signal_set(signal);
altb 8:dfe58484f580 73 }
altb 8:dfe58484f580 74
altb 8:dfe58484f580 75 void GPS::get_position(void){
altb 8:dfe58484f580 76 pos_xyz[0]=pos0_xyz[0];
altb 8:dfe58484f580 77 pos_xyz[1]=pos0_xyz[1];
altb 8:dfe58484f580 78 pos_xyz[2]=pos0_xyz[2];
altb 8:dfe58484f580 79 }
altb 8:dfe58484f580 80
altb 8:dfe58484f580 81
altb 8:dfe58484f580 82
altb 8:dfe58484f580 83 void GPS::Rx_interrupt() {
altb 8:dfe58484f580 84 while ((logGPS.readable()) ){
altb 8:dfe58484f580 85 buf[rx_in++] = logGPS.getc();
altb 8:dfe58484f580 86 if(rx_in==255)
altb 8:dfe58484f580 87 buffer_filled = true;
altb 8:dfe58484f580 88 }
altb 8:dfe58484f580 89 }
altb 8:dfe58484f580 90
altb 8:dfe58484f580 91 /*void GPS::Rx_interrupt2() {
altb 8:dfe58484f580 92 while(logGPS.readable() ){
altb 8:dfe58484f580 93 c = logGPS.getc();
altb 8:dfe58484f580 94 if(c == '$'){
altb 8:dfe58484f580 95 rx_in = 0;
altb 8:dfe58484f580 96 ready2analyse = false;
altb 8:dfe58484f580 97 for(uint8_t k=0;k<buffer_size;k++)
altb 8:dfe58484f580 98 buf[k]='-';
altb 8:dfe58484f580 99 }
altb 8:dfe58484f580 100 else if(rx_in == 0 && c == 'G')
altb 8:dfe58484f580 101 rx_in++;
altb 8:dfe58484f580 102 else if(rx_in == 1 && c == 'N')
altb 8:dfe58484f580 103 rx_in++;
altb 8:dfe58484f580 104 else if(rx_in == 2 && c == 'G')
altb 8:dfe58484f580 105 rx_in++;
altb 8:dfe58484f580 106 else if(rx_in == 3 && c == 'G')
altb 8:dfe58484f580 107 rx_in++;
altb 8:dfe58484f580 108 else if(rx_in == 4 && c == 'A')
altb 8:dfe58484f580 109 rx_in++;
altb 8:dfe58484f580 110 else if(rx_in > 4 && rx_in < buffer_size && is_analysed)
altb 8:dfe58484f580 111 rx_in++;
altb 8:dfe58484f580 112 buf[rx_in] = c;
altb 8:dfe58484f580 113 if(c== '*'){
altb 8:dfe58484f580 114 str = string(buf);
altb 8:dfe58484f580 115 str = str.substr(0,rx_in);
altb 8:dfe58484f580 116 ready2analyse = true;
altb 8:dfe58484f580 117 is_analysed = false;
altb 8:dfe58484f580 118 }
altb 8:dfe58484f580 119 }
altb 8:dfe58484f580 120 return;
altb 8:dfe58484f580 121 }*/
altb 8:dfe58484f580 122 void GPS::return_string(string *st){
altb 8:dfe58484f580 123 *st = string(buf);
altb 8:dfe58484f580 124 return;
altb 8:dfe58484f580 125
altb 8:dfe58484f580 126 }