richard misi
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;
}