altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Xtra_Sensors/GPS.cpp@2:e7874762cc25, 2019-10-21 (annotated)

Committer:
altb2
Date:
Mon Oct 21 17:16:11 2019 +0000
Revision:
2:e7874762cc25
Added additional Ekfs, tested AltHold (still some bugs, Problems at high Lift Rates)

Who changed what in which revision?

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