Ruprecht Altenburger / Misc_Lib

.

Dependents:  

GPS.cpp@9:33a8f794633c, 2018-12-04 (annotated)

Committer:
altb
Date:
Tue Dec 04 15:49:27 2018 +0000
Revision:
9:33a8f794633c
Parent:
8:dfe58484f580 
AHRS Klasse, kompiliert, aber nicht an QK getestet

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::return_string(string *st){
altb 8:dfe58484f580 92 *st = string(buf);
altb 8:dfe58484f580 93 return;
altb 8:dfe58484f580 94
altb 8:dfe58484f580 95 }