Essai GPS Fusion
Dependencies: BME280 BNO055 Eigen ds3231 mbed
essai-gps-fusion.cpp
- Committer:
- RCMISbed
- Date:
- 2018-10-22
- Revision:
- 1:03547f392fb7
- Parent:
- 0:07e03c88e90b
File content as of revision 1:03547f392fb7:
/* Test du module GPS */ #include "mbed.h" #include "time.h" #include "BME280.h" #include "BNO055.h" #include "ds3231.h" #include <Eigen/Dense.h> #include "objet.h" #include <stdio.h> #include <stdlib.h> #include <string> #include <iostream> #include <fstream> #include <sstream> using namespace std; using namespace Eigen; #define ON 1 #define OFF 0 // Definition/ Constantes/ Caracteres Alphanumeriques #define LF 10 // Line Feed \n #define CR 13 // Carriage Return \r #define BS 127 // Back Space // Definition/ Constantes/ Trigonométriques #define PI 3.14159265358979323846 // PI #define PI2 1.570796327 // PI/ 2 #define RD 57.29577951 // 180/ PI conversion radian ---> degre // Definition/ Constantes/ Geographiques Terrestres #define Rkm 6366.71 // Rayon moyen de la terre exprimé en KMs #define MAJA (6378137.0) // Axe principal de l'ellipsoid de référence #define FLAT (1.0/298.2572235630) // Coefficient de planéité de l'ellipsoid de référence #define ESQR (FLAT * (2.0-FLAT)) // WGS-84/ 1st eccentricity squared #define OMES (1.0 - ESQR) // WGS-84/ 1 minus eccentricity squared #define EFOR (ESQR * ESQR) // WGS-84/ Sqr of the 1st eccentricity squared #define ASQR (MAJA * MAJA) // WGS-84/ semi major axis squared = nlmaja** //Definition/ Constantes #define TSample 0.01 #define usbBR 115200 #define uartBR 38400 #define gpsBR 57600 #define i2cBR 400000 #define gpsWDTM 300 // Definition/ Constantes/ Dimension MAX Matrice #define matrixSMAX 4 // Matrice/ Dimension #define produit "GPS-Fusion-Kalman" #define NbEch 20 //Definition/ Variables Globales DigitalOut l1(LED1); DigitalOut l2(LED2); DigitalOut l3(LED3); DigitalOut l4(LED4); DigitalOut son(p8); Serial USB(USBTX, USBRX); // Liaison UART/ Dispositif TRAME USB Serial UART1 (p13, p14); // Liaison UART/ Dispositif TRAME HF Serial UART2 (p28, p27); // Liaison UART/ Dispositif GPS I2C i2c(p9, p10); // Liaison Bus I2C //BME280 pression(&i2c); BNO055 attitude(p9, p10); GPS_t gps; IMU_t imu; Ticker TMainProcess; string UART1Rx= ""; string UART2Rx= ""; char UART1Tx[100]; char gpsNMEA[100]; string USBRx = " "; char word[25]; bool prn= false; string trameID= "$GPRMC"; int pTRAME = 1; int l1WDT = 0; int l2WDT = 0; int l3WDT = 0; int l4WDT = 0; int gpsWDT = 0; int imuWDT = 0; int cT= 0; float utc= 0; string valid= "\0"; string id= ""; string val= "\0"; int cnt= 0; float signal= 0; float periode= 0.5; //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Identification Processus MatrixXd A(matrixSMAX,matrixSMAX); // Matrice/ Etat Systeme MatrixXd B(matrixSMAX,matrixSMAX); // Matrice/ Commande Systeme MatrixXd H(matrixSMAX,matrixSMAX); // Matrice/ Observation Systeme //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Covariance MatrixXd Q(matrixSMAX,matrixSMAX); // Matrice/ Covariance Bruit Systeme (perturbations) MatrixXd R(matrixSMAX,matrixSMAX); // Matrice/ Covariance Bruit Mesure //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Caractérisation du Filtre Kalman MatrixXd pXk(matrixSMAX,1); // Vecteur/ Etat a priori MatrixXd pPk(matrixSMAX,matrixSMAX); // Matrice/ Erreur Covariance a prori MatrixXd Xk(matrixSMAX,1); // Vecteur/ Etat MatrixXd Pk(matrixSMAX,matrixSMAX); // Matrice/ Erreur Covariance MatrixXd Kk(matrixSMAX,matrixSMAX); // Matrice/ Gain Kalman MatrixXd Zk(matrixSMAX,1); // Vecteur/ Mesure MatrixXd Uk(matrixSMAX,1); // Vecteur/ Commande MatrixXd I= MatrixXd::Identity(matrixSMAX,matrixSMAX); // Matrice/ Identité //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Identification Processus float An= 1, Bn= 0, Hn= 1, Qn= 0.00001, Rn= 0.1, pXn= 0, pPn= 0, Xn= 0, Yn= 0, Sn= 0, Pn= 1, Kn= 0, Zn= 0, Un= 0, In= 1; //Definition/ Procedures/ Reception UART1 (Interruption) //============================================================================== void Rx_UART1_Isr(void){ char RxCar; int pCar; int r1, r2, r3, r4, r5; RxCar = UART1.getc(); UART1Rx+= RxCar; USB.printf("%c", RxCar); if(UART1Rx.find("\r\n")< UART1Rx.length()){ //Balise fin de trame if(UART1Rx.find("$,123")< UART1Rx.length()){ l2WDT= 3; /* ++cnt; if(cnt> NbEch){cnt= 0;}; signal= 900* cos(((6.28* cnt)/NbEch)/(5.32*periode))* cos(((6.28* cnt)/NbEch)/periode); r1= signal; */ r1= (int)imu.roll; if(r1> 1000){r1= 1000;}; if(r1< -1000){r1= -1000;}; r2= (int)imu.pitch; if(r2> 1000){r2= 1000;}; if(r2< -1000){r2= -1000;}; r3= (int)imu.yaw; if(r3> 1000){r3= 1000;}; if(r3< -1000){r3= -1000;}; r4= 0; r5= 0; sprintf(UART1Tx,"$,123,%d,%d,%d,%d,%d,*\r\n",r1,r2,r3,r4,r5); UART1.printf("%s",UART1Tx); USB.printf("%s",UART1Tx); }; UART1Rx= ""; }; // fin if(UART2Rx.find("\r\n")< UART2Rx.length()){ } //Definition/ Procedures/ Reception UART2 (Interruption) //============================================================================== void Rx_UART2_Isr(void){ char RxCar; int pCar; int fx, nt; float vg, hd, dp; double ut, lt, lg, at, deg, min, dmin; string id, valid, ns, ew; RxCar = UART2.getc(); UART2Rx+= RxCar; if(UART2Rx.find("\r\n")< UART2Rx.length()){ //Balise fin de trame l1WDT= 3; pCar= 0;while(pCar< UART2Rx.length()){if(UART2Rx[pCar]== ','){UART2Rx.replace(pCar,1," ");};++pCar;}; //message: suppression des caracters ',' // Trame NMEA-083: $GPRMC if(UART2Rx.find("$GPRMC")< UART2Rx.length()){ // Example Trame: $GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68 ++cT; if(cT> 32000){cT= 0;}; // Totalisateur de Trame sprintf(gpsNMEA,"%s/ Trame NMEA (%d) : %s \r\n",produit,cT,UART2Rx); prn= true; // USB.printf("%s/ Trame NMEA (%d) : %s \r\n",produit, cT,UART2Rx); //DEBUG/ USB istringstream input(UART2Rx); input>> id>> ut>> valid>> lt>> ns>> lg>> ew>> vg>> hd; // USB.printf("%s/ Trame NMEA/ id : %s \r\n",produit, id); // GPS/ id // USB.printf("GPS/ Trame NMEA/ val : %s \r\n", valid); // GPS/ valid // USB.printf("GPS/ Trame NMEA/ utc : %f \r\n", ut); // GPS/ id // Convertion NED ---> ECEF (MERCATOE WGS84) gps.x= 0; gps.y= 0; gps.z= 0; if(valid== "A"){ //GPS/ data trame NMEA valides gpsWDT= gpsWDTM; gps.fValid= true; // USB.printf("%s/ Trame NMEA/ val : %s \r\n",produit, valid); // GPS/ valid // USB.printf("%s/ Trame NMEA/ utc : %f \r\n",produit, ut); // GPS/ id }; if(gpsWDT== 0){ // USB.printf("%s/ Trame NMEA/ val : INVALIDE !!! \r\n",produit); }; // GPS/ valid // USB.printf("\r\n\r\n\r\n"); }; // if(UART2Rx.find("$GPRMC")< UART2Rx.length()){ UART2Rx= ""; }; // fin if(UART2Rx.find("\r\n")< UART2Rx.length()){ } //Definition/ Procedures/ Reception des Trames serie (USB-UART Interruption) //============================================================================== void Rx_USB_Isr(void){ int cnt= 0; char car= USB.getc(); switch((int)car){ case 97: //Touche 'a' break; case 122: //Touche 'z' break; case BS://Back Space if(USBRx.length()> 0){ USBRx.erase(USBRx.length()-1,1); while(cnt< USBRx.length()){USB.printf(" ");++cnt;}; USB.printf("\r%s",USBRx); }; break; case LF: //Line Feed '\n' break; case CR: //Carriage Return '\r' USBRx+= "\r\n"; USB.printf("\r\n"); USBRx= ""; break; default: //Caractere ASCII USBRx+= car; USB.printf("%c",car); break; }; // fin switch((int)car){ } //Definition/ Procedures/ Processus Principal (Interruption) //============================================================================== void MainProcess_Isr(void){ //Watchdogs --l1WDT; if(l1WDT<= 0){l1WDT= 0; l1= OFF;}else{l1= ON;}; --l2WDT; if(l2WDT<= 0){l2WDT= 0; l2= OFF;}else{l2= ON;}; --l3WDT; if(l3WDT<= 0){l3WDT= 0; l3= OFF;}else{l3= ON;}; --l4WDT; if(l4WDT<= 0){l4WDT= 0; l4= OFF;}else{l4= ON;}; --imuWDT; if(imuWDT<= 0){imuWDT= 0;}; --gpsWDT; if(gpsWDT<= 0){ gpsWDT= 0; gps.fValid= false; gps.latitude= 0; gps.longitude= 0; gps.altitude= 0; gps.heading= 0; gps.vground= 0;}; //I2C/ imu if((imuWDT== 0)& (imu.fValid== true)){ attitude.get_calib(); attitude.get_lia(); // I2C/ IMU/ Acceleration Lineaire imu.liax = attitude.lia.x; imu.liay = attitude.lia.y; imu.liaz = attitude.lia.z; Zn= imu.roll; /* // Filtre Kalman/ Algorithme/ Prediction pXn = (An * Xn) + (Bn * Un); // 1- Equation Etat pPn = (An * Pn * An) + Qn; // 2- Equation Covariance de l'erreur // Filtre Kalman/ Algorithme/ Correction Kn = pPn * (Hn / ((Hn * pPn * Hn) + Rn)); // 1- Mise a jour Gain Kalman Xn = pXn + (Kn * (Zn - (Hn * pXn))); // 2- Mise a jour Etat Pn = (In - (Kn * Hn)) * pPn; // 3- Mise à jour Covariance de l'erreur */ pXn = Xn; pPn = Pn + Qn; Yn = Zn - pXn; Sn = pPn+ Rn; Kn = pPn/ Sn; Xn = pXn+ Kn*Yn; Pn = (In- Kn)*pPn; attitude.get_grv(); // I2C/ IMU/ Acceleration Gravite imu.grvx = attitude.gravity.x; imu.grvy = attitude.gravity.y; imu.grvz = attitude.gravity.z; attitude.get_gyro(); // I2C/ IMU/ Gyroscope imu.gyrox = attitude.gyro.x; imu.gyroy = attitude.gyro.y; imu.gyroz = attitude.gyro.z; attitude.get_mag(); // I2C/ IMU/ Champ Magnetique imu.magx = attitude.mag.x; imu.magy = attitude.mag.y; imu.magz = attitude.mag.z; attitude.get_angles(); // I2C/ IMU/ Angles EULER imu.roll = attitude.euler.roll; imu.pitch= attitude.euler.pitch; imu.yaw = attitude.euler.yaw; attitude.get_quat(); // I2C/ IMU/ Quarternions imu.q0 = attitude.quat.w; imu.q1 = attitude.quat.x; imu.q2 = attitude.quat.y; imu.q3 = attitude.quat.z; attitude.get_temp(); // I2C/ IMU/ Temperature imu.temperature = attitude.temperature; imuWDT= 20; };//fi if((imuWDT== 0)& (imu.fValid== true)){ /* //Localisation/ Fusion GPS-ACCELEROMETRE/ Fitrage Kalman (ref.2, ref.3) //Filtre/ Entree : Uk (vecteur Commande) //Filtre/ Entree : Zk (Vecteur Observabiité) //Filtre/ Sortie : Xk (Vecteur Etat) //Filtre/ Sortie : Pk (Vecteur Covariance) //Filtre/ Parametre: A (Matrice Etat) //Filtre/ Parametre: B (Matrice Commande) //Filtre/ Parametre: H (Matrice Observabilité) //Filtre/ Parametre: Q (Matrice Covariance Etat (bruit)) //Filtre/ Parametre: R (Matrice Covariance Observabilité (bruit)) // Filtre Kalman/ Algorithme/ Prediction pXk = (A * Xk) + (B * Uk); // 1- Equation Etat pPk = (A * Pk * A.transpose()) + Q; // 2- Equation Covariance de l'erreur // Filtre Kalman/ Algorithme/ Correction Kk = pPk * H.transpose() * ((H * pPk * H.transpose()) + R).inverse(); // 1- Mise a jour Gain Kalman Xk = pXk + (Kk * (Zk - (H * pXk))); // 2- Mise a jour Etat Pk = (I - (Kk * H)) * pPk; // 3- Mise à jour Covariance de l'erreur */ } //Definition/ Procedures/ Initialisation //============================================================================== int init_process(void){ int status= 0; USB.baud(usbBR); wait(0.3); UART1.baud(uartBR); wait(0.3); UART2.baud(gpsBR); wait(0.3); i2c.frequency(i2cBR); wait(0.2); // Initialisation/ Objet/ GPS gps.fValid = false; gps.wdtRX = gpsWDT; gps.baud = gpsBR; // gps.TSample = 5; gps.utc = 0; gps.vground = 0; gps.heading = 0; gps.fix = 0; gps.nbsat = 0; gps.dop = 0; gps.altitude = 0; gps.latitude = 0; gps.longitude = 0; gps.x = 0; gps.y = 0; gps.z = 0; gps.speedmin = 5; gps.Status = 0; // USB.printf("."); wait(0.3); // Initialisation/ Type/ objet IMU imu.fValid = true; imu.fFailed = false; imu.Status = 0; imu.roll = 0; imu.pitch = 0; imu.yaw = 0; imu.q0 = 1; imu.q1 = 0; imu.q2 = 0; imu.q3 = 0; imu.liax = 0; imu.liay = 0; imu.liaz = 0; imu.grvx = 0; imu.grvy = 0; imu.grvz = 0; imu.gyrox = 0; imu.gyroy = 0; imu.gyroz = 0; imu.magx = 0; imu.magy = 0; imu.pressure = 0; imu.humidity = 0; imu.temperature = 0; imu.altitude = 0; imu.magz = 0; imu.Status = 0; if(imu.fValid== true){ attitude.reset(); wait(1); if (!attitude.check()){ // Presence Capteur Attitude ? while (1){l1 =!l1; l2 =!l2; l3 =!l3; l4 =!l4; son= !son; wait(0.3);}; }; // Arret et flash LED et SOUND si le capteur d'attitude n'est pas reconnu // attitude.setpowermede(); // A definir attitude.set_accel_units(MILLIG); // Unite Accelleration MILLIG ou MPERSPERS attitude.set_anglerate_units(DEG_PER_SEC); // Unite Vitesse Angulaire RAD_PER_SEC ou DEG_PER_SEC attitude.set_angle_units(RADIANS); // Unite Angle RADIANS ou DEGREES attitude.set_temp_units(CENTIGRADE); // Unite Temperature CENTIGRADE ou FAHRENHEIT attitude.setmode(OPERATION_MODE_NDOF); // Mode Fonctionnement // if (pression.init() != 0) { // Presence Capteur pression atmosphérique ? // while (1){L1 =!L1; L2 =!L2; L3 =!L3; L4 =!L4; SOUND= !SOUND; wait(0.3);}; // }; // Arret et flash LED et SOUND si le capteur de pression n'est pas reconnu }; // fin if(imu.fvalid== true){ // USB.printf("."); wait(0.3); // Initialisation/ Objet/ MATRICE int cntr=0, cntc= 0; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< matrixSMAX){A(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< matrixSMAX){B(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< matrixSMAX){H(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< matrixSMAX){Q(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< matrixSMAX){R(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< 1){pXk(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< matrixSMAX){pPk(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< 1){Xk(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< matrixSMAX){Pk(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< matrixSMAX){Kk(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< 1){Zk(cntr,cntc)= 0;++cntc;};++cntr;}; cntr=0, cntc= 0; while(cntr< matrixSMAX){while(cntc< 1){Uk(cntr,cntc)= 0;++cntc;};++cntr;}; // Initialisation/ Objet/ Matrice Etat/ A A<< 1, TSample, 0, 0, 0, 1 , 0, 0, 0, 0 , 0, TSample, 0, 0 , 0, 1; // Initialisation/ Objet/ Matrice Commande/ B B<< 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; // Initialisation/ Objet/ Matrice Observabilite (Mesure)/ H H<< 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; // Initialisation/ Objet/ Matrice Covariance Etat/ P Pk<< 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; // Initialisation/ Objet/ Matrice Covariance Bruit Etat/ Q Q<< 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; // Initialisation/ Objet/ Matrice Covariance Bruit Observabilité (Mesure)/ R R<< 0.2, 0, 0, 0, 0, 0.2, 0, 0, 0, 0, 0.2, 0, 0, 0, 0, 0.2; // Initialisation/ Objet/ Matrice Identité/ I I<< 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; TMainProcess.attach(&MainProcess_Isr, TSample); UART1.attach(&Rx_UART1_Isr, Serial::RxIrq); UART2.attach(&Rx_UART2_Isr, Serial::RxIrq); USB.attach(&Rx_USB_Isr, Serial::RxIrq); son= 0; l1= OFF; l2= OFF; l3= OFF; l4= OFF; USB.printf("%s/ Essai ...\r\n",produit); wait(3); return status; } //Definition/ Procedures/ Procedure Principale //============================================================================== int main() { int status= 0; if(init_process()!= 0){l1=1, l2= 1; l3= 1; l4= 1; wait(0.3); l1=0, l2= 0; l3= 0; l4= 0; wait(0.7);}; while(1){}; return status; }