richard misi / Mbed 2 deprecated LPC1768-GPS-FUSION-17102018-1

Essai GPS Fusion

Dependencies:   BME280 BNO055 Eigen ds3231 mbed

essai-gps-fusion.cpp@1:03547f392fb7, 2018-10-22 (annotated)

Committer:
RCMISbed
Date:
Mon Oct 22 17:33:33 2018 +0000
Revision:
1:03547f392fb7
Parent:
0:07e03c88e90b 
gps fusion;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RCMISbed 0:07e03c88e90b 1 /*
RCMISbed 0:07e03c88e90b 2 Test du module GPS
RCMISbed 0:07e03c88e90b 3 */
RCMISbed 0:07e03c88e90b 4
RCMISbed 0:07e03c88e90b 5
RCMISbed 0:07e03c88e90b 6
RCMISbed 0:07e03c88e90b 7
RCMISbed 0:07e03c88e90b 8 #include "mbed.h"
RCMISbed 0:07e03c88e90b 9 #include "time.h"
RCMISbed 0:07e03c88e90b 10 #include "BME280.h"
RCMISbed 0:07e03c88e90b 11 #include "BNO055.h"
RCMISbed 0:07e03c88e90b 12 #include "ds3231.h"
RCMISbed 0:07e03c88e90b 13 #include <Eigen/Dense.h>
RCMISbed 0:07e03c88e90b 14 #include "objet.h"
RCMISbed 0:07e03c88e90b 15 #include <stdio.h>
RCMISbed 0:07e03c88e90b 16 #include <stdlib.h>
RCMISbed 0:07e03c88e90b 17 #include <string>
RCMISbed 0:07e03c88e90b 18 #include <iostream>
RCMISbed 0:07e03c88e90b 19 #include <fstream>
RCMISbed 0:07e03c88e90b 20 #include <sstream>
RCMISbed 0:07e03c88e90b 21
RCMISbed 0:07e03c88e90b 22 using namespace std;
RCMISbed 0:07e03c88e90b 23 using namespace Eigen;
RCMISbed 0:07e03c88e90b 24
RCMISbed 0:07e03c88e90b 25 #define ON 1
RCMISbed 0:07e03c88e90b 26 #define OFF 0
RCMISbed 0:07e03c88e90b 27
RCMISbed 0:07e03c88e90b 28 // Definition/ Constantes/ Caracteres Alphanumeriques
RCMISbed 0:07e03c88e90b 29 #define LF 10 // Line Feed \n
RCMISbed 0:07e03c88e90b 30 #define CR 13 // Carriage Return \r
RCMISbed 0:07e03c88e90b 31 #define BS 127 // Back Space
RCMISbed 0:07e03c88e90b 32
RCMISbed 0:07e03c88e90b 33 // Definition/ Constantes/ Trigonométriques
RCMISbed 0:07e03c88e90b 34 #define PI 3.14159265358979323846 // PI
RCMISbed 0:07e03c88e90b 35 #define PI2 1.570796327 // PI/ 2
RCMISbed 0:07e03c88e90b 36 #define RD 57.29577951 // 180/ PI conversion radian ---> degre
RCMISbed 0:07e03c88e90b 37
RCMISbed 0:07e03c88e90b 38 // Definition/ Constantes/ Geographiques Terrestres
RCMISbed 0:07e03c88e90b 39 #define Rkm 6366.71 // Rayon moyen de la terre exprimé en KMs
RCMISbed 0:07e03c88e90b 40 #define MAJA (6378137.0) // Axe principal de l'ellipsoid de référence
RCMISbed 0:07e03c88e90b 41 #define FLAT (1.0/298.2572235630) // Coefficient de planéité de l'ellipsoid de référence
RCMISbed 0:07e03c88e90b 42 #define ESQR (FLAT * (2.0-FLAT)) // WGS-84/ 1st eccentricity squared
RCMISbed 0:07e03c88e90b 43 #define OMES (1.0 - ESQR) // WGS-84/ 1 minus eccentricity squared
RCMISbed 0:07e03c88e90b 44 #define EFOR (ESQR * ESQR) // WGS-84/ Sqr of the 1st eccentricity squared
RCMISbed 0:07e03c88e90b 45 #define ASQR (MAJA * MAJA) // WGS-84/ semi major axis squared = nlmaja**
RCMISbed 0:07e03c88e90b 46
RCMISbed 0:07e03c88e90b 47 //Definition/ Constantes
RCMISbed 0:07e03c88e90b 48 #define TSample 0.01
RCMISbed 0:07e03c88e90b 49 #define usbBR 115200
RCMISbed 0:07e03c88e90b 50 #define uartBR 38400
RCMISbed 0:07e03c88e90b 51 #define gpsBR 57600
RCMISbed 0:07e03c88e90b 52 #define i2cBR 400000
RCMISbed 0:07e03c88e90b 53 #define gpsWDTM 300
RCMISbed 0:07e03c88e90b 54
RCMISbed 0:07e03c88e90b 55 // Definition/ Constantes/ Dimension MAX Matrice
RCMISbed 0:07e03c88e90b 56 #define matrixSMAX 4 // Matrice/ Dimension
RCMISbed 0:07e03c88e90b 57
RCMISbed 0:07e03c88e90b 58 #define produit "GPS-Fusion-Kalman"
RCMISbed 0:07e03c88e90b 59
RCMISbed 0:07e03c88e90b 60 #define NbEch 20
RCMISbed 0:07e03c88e90b 61
RCMISbed 0:07e03c88e90b 62 //Definition/ Variables Globales
RCMISbed 0:07e03c88e90b 63 DigitalOut l1(LED1);
RCMISbed 0:07e03c88e90b 64 DigitalOut l2(LED2);
RCMISbed 0:07e03c88e90b 65 DigitalOut l3(LED3);
RCMISbed 0:07e03c88e90b 66 DigitalOut l4(LED4);
RCMISbed 0:07e03c88e90b 67 DigitalOut son(p8);
RCMISbed 0:07e03c88e90b 68 Serial USB(USBTX, USBRX); // Liaison UART/ Dispositif TRAME USB
RCMISbed 0:07e03c88e90b 69 Serial UART1 (p13, p14); // Liaison UART/ Dispositif TRAME HF
RCMISbed 0:07e03c88e90b 70 Serial UART2 (p28, p27); // Liaison UART/ Dispositif GPS
RCMISbed 0:07e03c88e90b 71 I2C i2c(p9, p10); // Liaison Bus I2C
RCMISbed 0:07e03c88e90b 72 //BME280 pression(&i2c);
RCMISbed 0:07e03c88e90b 73 BNO055 attitude(p9, p10);
RCMISbed 0:07e03c88e90b 74 GPS_t gps;
RCMISbed 0:07e03c88e90b 75 IMU_t imu;
RCMISbed 0:07e03c88e90b 76 Ticker TMainProcess;
RCMISbed 0:07e03c88e90b 77 string UART1Rx= "";
RCMISbed 0:07e03c88e90b 78 string UART2Rx= "";
RCMISbed 0:07e03c88e90b 79 char UART1Tx[100];
RCMISbed 0:07e03c88e90b 80 char gpsNMEA[100];
RCMISbed 0:07e03c88e90b 81 string USBRx = " ";
RCMISbed 0:07e03c88e90b 82 char word[25];
RCMISbed 0:07e03c88e90b 83 bool prn= false;
RCMISbed 0:07e03c88e90b 84
RCMISbed 0:07e03c88e90b 85 string trameID= "$GPRMC";
RCMISbed 0:07e03c88e90b 86 int pTRAME = 1;
RCMISbed 0:07e03c88e90b 87 int l1WDT = 0;
RCMISbed 0:07e03c88e90b 88 int l2WDT = 0;
RCMISbed 0:07e03c88e90b 89 int l3WDT = 0;
RCMISbed 0:07e03c88e90b 90 int l4WDT = 0;
RCMISbed 0:07e03c88e90b 91 int gpsWDT = 0;
RCMISbed 0:07e03c88e90b 92 int imuWDT = 0;
RCMISbed 0:07e03c88e90b 93
RCMISbed 0:07e03c88e90b 94 int cT= 0;
RCMISbed 0:07e03c88e90b 95
RCMISbed 0:07e03c88e90b 96 float utc= 0;
RCMISbed 0:07e03c88e90b 97 string valid= "\0";
RCMISbed 0:07e03c88e90b 98 string id= "";
RCMISbed 0:07e03c88e90b 99 string val= "\0";
RCMISbed 0:07e03c88e90b 100
RCMISbed 0:07e03c88e90b 101
RCMISbed 0:07e03c88e90b 102 int cnt= 0;
RCMISbed 0:07e03c88e90b 103 float signal= 0;
RCMISbed 0:07e03c88e90b 104 float periode= 0.5;
RCMISbed 0:07e03c88e90b 105
RCMISbed 0:07e03c88e90b 106
RCMISbed 0:07e03c88e90b 107 //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Identification Processus
RCMISbed 0:07e03c88e90b 108 MatrixXd A(matrixSMAX,matrixSMAX); // Matrice/ Etat Systeme
RCMISbed 0:07e03c88e90b 109 MatrixXd B(matrixSMAX,matrixSMAX); // Matrice/ Commande Systeme
RCMISbed 0:07e03c88e90b 110 MatrixXd H(matrixSMAX,matrixSMAX); // Matrice/ Observation Systeme
RCMISbed 0:07e03c88e90b 111 //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Covariance
RCMISbed 0:07e03c88e90b 112 MatrixXd Q(matrixSMAX,matrixSMAX); // Matrice/ Covariance Bruit Systeme (perturbations)
RCMISbed 0:07e03c88e90b 113 MatrixXd R(matrixSMAX,matrixSMAX); // Matrice/ Covariance Bruit Mesure
RCMISbed 0:07e03c88e90b 114 //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Caractérisation du Filtre Kalman
RCMISbed 0:07e03c88e90b 115 MatrixXd pXk(matrixSMAX,1); // Vecteur/ Etat a priori
RCMISbed 0:07e03c88e90b 116 MatrixXd pPk(matrixSMAX,matrixSMAX); // Matrice/ Erreur Covariance a prori
RCMISbed 0:07e03c88e90b 117 MatrixXd Xk(matrixSMAX,1); // Vecteur/ Etat
RCMISbed 0:07e03c88e90b 118 MatrixXd Pk(matrixSMAX,matrixSMAX); // Matrice/ Erreur Covariance
RCMISbed 0:07e03c88e90b 119 MatrixXd Kk(matrixSMAX,matrixSMAX); // Matrice/ Gain Kalman
RCMISbed 0:07e03c88e90b 120 MatrixXd Zk(matrixSMAX,1); // Vecteur/ Mesure
RCMISbed 0:07e03c88e90b 121 MatrixXd Uk(matrixSMAX,1); // Vecteur/ Commande
RCMISbed 0:07e03c88e90b 122 MatrixXd I= MatrixXd::Identity(matrixSMAX,matrixSMAX); // Matrice/ Identité
RCMISbed 0:07e03c88e90b 123
RCMISbed 0:07e03c88e90b 124
RCMISbed 0:07e03c88e90b 125
RCMISbed 0:07e03c88e90b 126
RCMISbed 0:07e03c88e90b 127 //Definition/ Variables Globales/ Processus Principal/ Variables Matricielles/ Identification Processus
RCMISbed 0:07e03c88e90b 128 float An= 1,
RCMISbed 0:07e03c88e90b 129 Bn= 0,
RCMISbed 0:07e03c88e90b 130 Hn= 1,
RCMISbed 0:07e03c88e90b 131 Qn= 0.00001,
RCMISbed 0:07e03c88e90b 132 Rn= 0.1,
RCMISbed 0:07e03c88e90b 133 pXn= 0,
RCMISbed 0:07e03c88e90b 134 pPn= 0,
RCMISbed 0:07e03c88e90b 135 Xn= 0,
RCMISbed 0:07e03c88e90b 136 Yn= 0,
RCMISbed 0:07e03c88e90b 137 Sn= 0,
RCMISbed 0:07e03c88e90b 138 Pn= 1,
RCMISbed 0:07e03c88e90b 139 Kn= 0,
RCMISbed 0:07e03c88e90b 140 Zn= 0,
RCMISbed 0:07e03c88e90b 141 Un= 0,
RCMISbed 0:07e03c88e90b 142 In= 1;
RCMISbed 0:07e03c88e90b 143
RCMISbed 0:07e03c88e90b 144
RCMISbed 0:07e03c88e90b 145 //Definition/ Procedures/ Reception UART1 (Interruption)
RCMISbed 0:07e03c88e90b 146 //==============================================================================
RCMISbed 0:07e03c88e90b 147 void Rx_UART1_Isr(void){
RCMISbed 0:07e03c88e90b 148
RCMISbed 0:07e03c88e90b 149 char RxCar;
RCMISbed 0:07e03c88e90b 150 int pCar;
RCMISbed 0:07e03c88e90b 151 int r1, r2, r3, r4, r5;
RCMISbed 0:07e03c88e90b 152
RCMISbed 0:07e03c88e90b 153 RxCar = UART1.getc();
RCMISbed 0:07e03c88e90b 154 UART1Rx+= RxCar;
RCMISbed 0:07e03c88e90b 155
RCMISbed 0:07e03c88e90b 156 USB.printf("%c", RxCar);
RCMISbed 0:07e03c88e90b 157
RCMISbed 0:07e03c88e90b 158 if(UART1Rx.find("\r\n")< UART1Rx.length()){ //Balise fin de trame
RCMISbed 0:07e03c88e90b 159 if(UART1Rx.find("$,123")< UART1Rx.length()){
RCMISbed 0:07e03c88e90b 160 l2WDT= 3;
RCMISbed 0:07e03c88e90b 161
RCMISbed 0:07e03c88e90b 162 /* ++cnt; if(cnt> NbEch){cnt= 0;};
RCMISbed 0:07e03c88e90b 163 signal= 900* cos(((6.28* cnt)/NbEch)/(5.32*periode))* cos(((6.28* cnt)/NbEch)/periode);
RCMISbed 0:07e03c88e90b 164 r1= signal;
RCMISbed 0:07e03c88e90b 165 */
RCMISbed 0:07e03c88e90b 166 r1= (int)imu.roll; if(r1> 1000){r1= 1000;}; if(r1< -1000){r1= -1000;};
RCMISbed 0:07e03c88e90b 167 r2= (int)imu.pitch; if(r2> 1000){r2= 1000;}; if(r2< -1000){r2= -1000;};
RCMISbed 0:07e03c88e90b 168 r3= (int)imu.yaw; if(r3> 1000){r3= 1000;}; if(r3< -1000){r3= -1000;};
RCMISbed 0:07e03c88e90b 169 r4= 0;
RCMISbed 0:07e03c88e90b 170 r5= 0;
RCMISbed 0:07e03c88e90b 171 sprintf(UART1Tx,"$,123,%d,%d,%d,%d,%d,*\r\n",r1,r2,r3,r4,r5);
RCMISbed 0:07e03c88e90b 172 UART1.printf("%s",UART1Tx);
RCMISbed 0:07e03c88e90b 173 USB.printf("%s",UART1Tx);
RCMISbed 0:07e03c88e90b 174 };
RCMISbed 0:07e03c88e90b 175 UART1Rx= "";
RCMISbed 0:07e03c88e90b 176 }; // fin if(UART2Rx.find("\r\n")< UART2Rx.length()){
RCMISbed 0:07e03c88e90b 177 }
RCMISbed 0:07e03c88e90b 178
RCMISbed 0:07e03c88e90b 179
RCMISbed 0:07e03c88e90b 180
RCMISbed 0:07e03c88e90b 181
RCMISbed 0:07e03c88e90b 182
RCMISbed 0:07e03c88e90b 183 //Definition/ Procedures/ Reception UART2 (Interruption)
RCMISbed 0:07e03c88e90b 184 //==============================================================================
RCMISbed 0:07e03c88e90b 185 void Rx_UART2_Isr(void){
RCMISbed 0:07e03c88e90b 186
RCMISbed 0:07e03c88e90b 187 char RxCar;
RCMISbed 0:07e03c88e90b 188 int pCar;
RCMISbed 0:07e03c88e90b 189 int fx, nt;
RCMISbed 0:07e03c88e90b 190 float vg, hd, dp;
RCMISbed 0:07e03c88e90b 191 double ut, lt, lg, at, deg, min, dmin;
RCMISbed 0:07e03c88e90b 192 string id, valid, ns, ew;
RCMISbed 0:07e03c88e90b 193
RCMISbed 0:07e03c88e90b 194 RxCar = UART2.getc();
RCMISbed 0:07e03c88e90b 195 UART2Rx+= RxCar;
RCMISbed 0:07e03c88e90b 196
RCMISbed 0:07e03c88e90b 197 if(UART2Rx.find("\r\n")< UART2Rx.length()){ //Balise fin de trame
RCMISbed 0:07e03c88e90b 198 l1WDT= 3;
RCMISbed 0:07e03c88e90b 199
RCMISbed 0:07e03c88e90b 200 pCar= 0;while(pCar< UART2Rx.length()){if(UART2Rx[pCar]== ','){UART2Rx.replace(pCar,1," ");};++pCar;}; //message: suppression des caracters ','
RCMISbed 0:07e03c88e90b 201
RCMISbed 0:07e03c88e90b 202 // Trame NMEA-083: $GPRMC
RCMISbed 0:07e03c88e90b 203 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
RCMISbed 0:07e03c88e90b 204 ++cT; if(cT> 32000){cT= 0;}; // Totalisateur de Trame
RCMISbed 0:07e03c88e90b 205 sprintf(gpsNMEA,"%s/ Trame NMEA (%d) : %s \r\n",produit,cT,UART2Rx); prn= true;
RCMISbed 0:07e03c88e90b 206 // USB.printf("%s/ Trame NMEA (%d) : %s \r\n",produit, cT,UART2Rx); //DEBUG/ USB
RCMISbed 0:07e03c88e90b 207 istringstream input(UART2Rx);
RCMISbed 0:07e03c88e90b 208 input>> id>> ut>> valid>> lt>> ns>> lg>> ew>> vg>> hd;
RCMISbed 0:07e03c88e90b 209 // USB.printf("%s/ Trame NMEA/ id : %s \r\n",produit, id); // GPS/ id
RCMISbed 0:07e03c88e90b 210 // USB.printf("GPS/ Trame NMEA/ val : %s \r\n", valid); // GPS/ valid
RCMISbed 0:07e03c88e90b 211 // USB.printf("GPS/ Trame NMEA/ utc : %f \r\n", ut); // GPS/ id
RCMISbed 0:07e03c88e90b 212 // Convertion NED ---> ECEF (MERCATOE WGS84)
RCMISbed 0:07e03c88e90b 213 gps.x= 0;
RCMISbed 0:07e03c88e90b 214 gps.y= 0;
RCMISbed 0:07e03c88e90b 215 gps.z= 0;
RCMISbed 0:07e03c88e90b 216 if(valid== "A"){ //GPS/ data trame NMEA valides
RCMISbed 0:07e03c88e90b 217 gpsWDT= gpsWDTM;
RCMISbed 0:07e03c88e90b 218 gps.fValid= true;
RCMISbed 0:07e03c88e90b 219 // USB.printf("%s/ Trame NMEA/ val : %s \r\n",produit, valid); // GPS/ valid
RCMISbed 0:07e03c88e90b 220 // USB.printf("%s/ Trame NMEA/ utc : %f \r\n",produit, ut); // GPS/ id
RCMISbed 0:07e03c88e90b 221 };
RCMISbed 0:07e03c88e90b 222 if(gpsWDT== 0){
RCMISbed 0:07e03c88e90b 223 // USB.printf("%s/ Trame NMEA/ val : INVALIDE !!! \r\n",produit);
RCMISbed 0:07e03c88e90b 224 }; // GPS/ valid
RCMISbed 0:07e03c88e90b 225
RCMISbed 0:07e03c88e90b 226 // USB.printf("\r\n\r\n\r\n");
RCMISbed 0:07e03c88e90b 227 }; // if(UART2Rx.find("$GPRMC")< UART2Rx.length()){
RCMISbed 0:07e03c88e90b 228
RCMISbed 0:07e03c88e90b 229 UART2Rx= "";
RCMISbed 0:07e03c88e90b 230 }; // fin if(UART2Rx.find("\r\n")< UART2Rx.length()){
RCMISbed 0:07e03c88e90b 231 }
RCMISbed 0:07e03c88e90b 232
RCMISbed 0:07e03c88e90b 233
RCMISbed 0:07e03c88e90b 234
RCMISbed 0:07e03c88e90b 235
RCMISbed 0:07e03c88e90b 236
RCMISbed 0:07e03c88e90b 237
RCMISbed 0:07e03c88e90b 238 //Definition/ Procedures/ Reception des Trames serie (USB-UART Interruption)
RCMISbed 0:07e03c88e90b 239 //==============================================================================
RCMISbed 0:07e03c88e90b 240 void Rx_USB_Isr(void){
RCMISbed 0:07e03c88e90b 241 int cnt= 0;
RCMISbed 0:07e03c88e90b 242 char car= USB.getc();
RCMISbed 0:07e03c88e90b 243
RCMISbed 0:07e03c88e90b 244 switch((int)car){
RCMISbed 0:07e03c88e90b 245
RCMISbed 0:07e03c88e90b 246 case 97: //Touche 'a'
RCMISbed 0:07e03c88e90b 247 break;
RCMISbed 0:07e03c88e90b 248 case 122: //Touche 'z'
RCMISbed 0:07e03c88e90b 249 break;
RCMISbed 0:07e03c88e90b 250 case BS://Back Space
RCMISbed 0:07e03c88e90b 251 if(USBRx.length()> 0){
RCMISbed 0:07e03c88e90b 252 USBRx.erase(USBRx.length()-1,1);
RCMISbed 0:07e03c88e90b 253 while(cnt< USBRx.length()){USB.printf(" ");++cnt;};
RCMISbed 0:07e03c88e90b 254 USB.printf("\r%s",USBRx);
RCMISbed 0:07e03c88e90b 255 };
RCMISbed 0:07e03c88e90b 256 break;
RCMISbed 0:07e03c88e90b 257 case LF: //Line Feed '\n'
RCMISbed 0:07e03c88e90b 258 break;
RCMISbed 0:07e03c88e90b 259 case CR: //Carriage Return '\r'
RCMISbed 0:07e03c88e90b 260 USBRx+= "\r\n";
RCMISbed 0:07e03c88e90b 261 USB.printf("\r\n");
RCMISbed 0:07e03c88e90b 262 USBRx= "";
RCMISbed 0:07e03c88e90b 263 break;
RCMISbed 0:07e03c88e90b 264 default: //Caractere ASCII
RCMISbed 0:07e03c88e90b 265 USBRx+= car;
RCMISbed 0:07e03c88e90b 266 USB.printf("%c",car);
RCMISbed 0:07e03c88e90b 267 break;
RCMISbed 0:07e03c88e90b 268 }; // fin switch((int)car){
RCMISbed 0:07e03c88e90b 269
RCMISbed 0:07e03c88e90b 270 }
RCMISbed 0:07e03c88e90b 271
RCMISbed 0:07e03c88e90b 272
RCMISbed 0:07e03c88e90b 273
RCMISbed 0:07e03c88e90b 274
RCMISbed 0:07e03c88e90b 275
RCMISbed 0:07e03c88e90b 276
RCMISbed 0:07e03c88e90b 277
RCMISbed 0:07e03c88e90b 278
RCMISbed 0:07e03c88e90b 279
RCMISbed 0:07e03c88e90b 280 //Definition/ Procedures/ Processus Principal (Interruption)
RCMISbed 0:07e03c88e90b 281 //==============================================================================
RCMISbed 0:07e03c88e90b 282 void MainProcess_Isr(void){
RCMISbed 0:07e03c88e90b 283 //Watchdogs
RCMISbed 0:07e03c88e90b 284 --l1WDT; if(l1WDT<= 0){l1WDT= 0; l1= OFF;}else{l1= ON;};
RCMISbed 0:07e03c88e90b 285 --l2WDT; if(l2WDT<= 0){l2WDT= 0; l2= OFF;}else{l2= ON;};
RCMISbed 0:07e03c88e90b 286 --l3WDT; if(l3WDT<= 0){l3WDT= 0; l3= OFF;}else{l3= ON;};
RCMISbed 0:07e03c88e90b 287 --l4WDT; if(l4WDT<= 0){l4WDT= 0; l4= OFF;}else{l4= ON;};
RCMISbed 0:07e03c88e90b 288 --imuWDT; if(imuWDT<= 0){imuWDT= 0;};
RCMISbed 0:07e03c88e90b 289 --gpsWDT; if(gpsWDT<= 0){
RCMISbed 0:07e03c88e90b 290 gpsWDT= 0;
RCMISbed 0:07e03c88e90b 291 gps.fValid= false;
RCMISbed 0:07e03c88e90b 292 gps.latitude= 0; gps.longitude= 0; gps.altitude= 0;
RCMISbed 0:07e03c88e90b 293 gps.heading= 0; gps.vground= 0;};
RCMISbed 0:07e03c88e90b 294
RCMISbed 0:07e03c88e90b 295 //I2C/ imu
RCMISbed 0:07e03c88e90b 296 if((imuWDT== 0)& (imu.fValid== true)){
RCMISbed 0:07e03c88e90b 297 attitude.get_calib();
RCMISbed 0:07e03c88e90b 298
RCMISbed 0:07e03c88e90b 299 attitude.get_lia(); // I2C/ IMU/ Acceleration Lineaire
RCMISbed 0:07e03c88e90b 300 imu.liax = attitude.lia.x;
RCMISbed 0:07e03c88e90b 301 imu.liay = attitude.lia.y;
RCMISbed 0:07e03c88e90b 302 imu.liaz = attitude.lia.z;
RCMISbed 0:07e03c88e90b 303
RCMISbed 0:07e03c88e90b 304
RCMISbed 0:07e03c88e90b 305 Zn= imu.roll;
RCMISbed 0:07e03c88e90b 306 /*
RCMISbed 0:07e03c88e90b 307 // Filtre Kalman/ Algorithme/ Prediction
RCMISbed 0:07e03c88e90b 308 pXn = (An * Xn) + (Bn * Un); // 1- Equation Etat
RCMISbed 0:07e03c88e90b 309 pPn = (An * Pn * An) + Qn; // 2- Equation Covariance de l'erreur
RCMISbed 0:07e03c88e90b 310 // Filtre Kalman/ Algorithme/ Correction
RCMISbed 0:07e03c88e90b 311 Kn = pPn * (Hn / ((Hn * pPn * Hn) + Rn)); // 1- Mise a jour Gain Kalman
RCMISbed 0:07e03c88e90b 312 Xn = pXn + (Kn * (Zn - (Hn * pXn))); // 2- Mise a jour Etat
RCMISbed 0:07e03c88e90b 313 Pn = (In - (Kn * Hn)) * pPn; // 3- Mise à jour Covariance de l'erreur
RCMISbed 0:07e03c88e90b 314 */
RCMISbed 0:07e03c88e90b 315
RCMISbed 0:07e03c88e90b 316 pXn = Xn;
RCMISbed 0:07e03c88e90b 317 pPn = Pn + Qn;
RCMISbed 0:07e03c88e90b 318 Yn = Zn - pXn;
RCMISbed 0:07e03c88e90b 319 Sn = pPn+ Rn;
RCMISbed 0:07e03c88e90b 320 Kn = pPn/ Sn;
RCMISbed 0:07e03c88e90b 321 Xn = pXn+ Kn*Yn;
RCMISbed 0:07e03c88e90b 322 Pn = (In- Kn)*pPn;
RCMISbed 0:07e03c88e90b 323
RCMISbed 0:07e03c88e90b 324 attitude.get_grv(); // I2C/ IMU/ Acceleration Gravite
RCMISbed 0:07e03c88e90b 325 imu.grvx = attitude.gravity.x;
RCMISbed 0:07e03c88e90b 326 imu.grvy = attitude.gravity.y;
RCMISbed 0:07e03c88e90b 327 imu.grvz = attitude.gravity.z;
RCMISbed 0:07e03c88e90b 328
RCMISbed 0:07e03c88e90b 329 attitude.get_gyro(); // I2C/ IMU/ Gyroscope
RCMISbed 0:07e03c88e90b 330 imu.gyrox = attitude.gyro.x;
RCMISbed 0:07e03c88e90b 331 imu.gyroy = attitude.gyro.y;
RCMISbed 0:07e03c88e90b 332 imu.gyroz = attitude.gyro.z;
RCMISbed 0:07e03c88e90b 333
RCMISbed 0:07e03c88e90b 334 attitude.get_mag(); // I2C/ IMU/ Champ Magnetique
RCMISbed 0:07e03c88e90b 335 imu.magx = attitude.mag.x;
RCMISbed 0:07e03c88e90b 336 imu.magy = attitude.mag.y;
RCMISbed 0:07e03c88e90b 337 imu.magz = attitude.mag.z;
RCMISbed 0:07e03c88e90b 338
RCMISbed 0:07e03c88e90b 339 attitude.get_angles(); // I2C/ IMU/ Angles EULER
RCMISbed 0:07e03c88e90b 340 imu.roll = attitude.euler.roll;
RCMISbed 0:07e03c88e90b 341 imu.pitch= attitude.euler.pitch;
RCMISbed 0:07e03c88e90b 342 imu.yaw = attitude.euler.yaw;
RCMISbed 0:07e03c88e90b 343
RCMISbed 0:07e03c88e90b 344 attitude.get_quat(); // I2C/ IMU/ Quarternions
RCMISbed 0:07e03c88e90b 345 imu.q0 = attitude.quat.w;
RCMISbed 0:07e03c88e90b 346 imu.q1 = attitude.quat.x;
RCMISbed 0:07e03c88e90b 347 imu.q2 = attitude.quat.y;
RCMISbed 0:07e03c88e90b 348 imu.q3 = attitude.quat.z;
RCMISbed 0:07e03c88e90b 349
RCMISbed 0:07e03c88e90b 350 attitude.get_temp(); // I2C/ IMU/ Temperature
RCMISbed 0:07e03c88e90b 351 imu.temperature = attitude.temperature;
RCMISbed 0:07e03c88e90b 352
RCMISbed 0:07e03c88e90b 353 imuWDT= 20;
RCMISbed 0:07e03c88e90b 354
RCMISbed 0:07e03c88e90b 355 };//fi if((imuWDT== 0)& (imu.fValid== true)){
RCMISbed 0:07e03c88e90b 356
RCMISbed 0:07e03c88e90b 357 /*
RCMISbed 0:07e03c88e90b 358 //Localisation/ Fusion GPS-ACCELEROMETRE/ Fitrage Kalman (ref.2, ref.3)
RCMISbed 0:07e03c88e90b 359 //Filtre/ Entree : Uk (vecteur Commande)
RCMISbed 0:07e03c88e90b 360 //Filtre/ Entree : Zk (Vecteur Observabiité)
RCMISbed 0:07e03c88e90b 361 //Filtre/ Sortie : Xk (Vecteur Etat)
RCMISbed 0:07e03c88e90b 362 //Filtre/ Sortie : Pk (Vecteur Covariance)
RCMISbed 0:07e03c88e90b 363 //Filtre/ Parametre: A (Matrice Etat)
RCMISbed 0:07e03c88e90b 364 //Filtre/ Parametre: B (Matrice Commande)
RCMISbed 0:07e03c88e90b 365 //Filtre/ Parametre: H (Matrice Observabilité)
RCMISbed 0:07e03c88e90b 366 //Filtre/ Parametre: Q (Matrice Covariance Etat (bruit))
RCMISbed 0:07e03c88e90b 367 //Filtre/ Parametre: R (Matrice Covariance Observabilité (bruit))
RCMISbed 0:07e03c88e90b 368
RCMISbed 0:07e03c88e90b 369 // Filtre Kalman/ Algorithme/ Prediction
RCMISbed 0:07e03c88e90b 370 pXk = (A * Xk) + (B * Uk); // 1- Equation Etat
RCMISbed 0:07e03c88e90b 371 pPk = (A * Pk * A.transpose()) + Q; // 2- Equation Covariance de l'erreur
RCMISbed 0:07e03c88e90b 372 // Filtre Kalman/ Algorithme/ Correction
RCMISbed 0:07e03c88e90b 373 Kk = pPk * H.transpose() * ((H * pPk * H.transpose()) + R).inverse(); // 1- Mise a jour Gain Kalman
RCMISbed 0:07e03c88e90b 374 Xk = pXk + (Kk * (Zk - (H * pXk))); // 2- Mise a jour Etat
RCMISbed 0:07e03c88e90b 375 Pk = (I - (Kk * H)) * pPk; // 3- Mise à jour Covariance de l'erreur
RCMISbed 0:07e03c88e90b 376 */
RCMISbed 0:07e03c88e90b 377
RCMISbed 0:07e03c88e90b 378 }
RCMISbed 0:07e03c88e90b 379
RCMISbed 0:07e03c88e90b 380
RCMISbed 0:07e03c88e90b 381
RCMISbed 0:07e03c88e90b 382
RCMISbed 0:07e03c88e90b 383
RCMISbed 0:07e03c88e90b 384
RCMISbed 0:07e03c88e90b 385
RCMISbed 0:07e03c88e90b 386 //Definition/ Procedures/ Initialisation
RCMISbed 0:07e03c88e90b 387 //==============================================================================
RCMISbed 0:07e03c88e90b 388 int init_process(void){
RCMISbed 0:07e03c88e90b 389 int status= 0;
RCMISbed 0:07e03c88e90b 390 USB.baud(usbBR); wait(0.3);
RCMISbed 0:07e03c88e90b 391 UART1.baud(uartBR); wait(0.3);
RCMISbed 0:07e03c88e90b 392 UART2.baud(gpsBR); wait(0.3);
RCMISbed 0:07e03c88e90b 393 i2c.frequency(i2cBR); wait(0.2);
RCMISbed 0:07e03c88e90b 394
RCMISbed 0:07e03c88e90b 395 // Initialisation/ Objet/ GPS
RCMISbed 0:07e03c88e90b 396 gps.fValid = false;
RCMISbed 0:07e03c88e90b 397 gps.wdtRX = gpsWDT;
RCMISbed 0:07e03c88e90b 398 gps.baud = gpsBR;
RCMISbed 0:07e03c88e90b 399 // gps.TSample = 5;
RCMISbed 0:07e03c88e90b 400 gps.utc = 0;
RCMISbed 0:07e03c88e90b 401 gps.vground = 0;
RCMISbed 0:07e03c88e90b 402 gps.heading = 0;
RCMISbed 0:07e03c88e90b 403 gps.fix = 0;
RCMISbed 0:07e03c88e90b 404 gps.nbsat = 0;
RCMISbed 0:07e03c88e90b 405 gps.dop = 0;
RCMISbed 0:07e03c88e90b 406 gps.altitude = 0;
RCMISbed 0:07e03c88e90b 407 gps.latitude = 0;
RCMISbed 0:07e03c88e90b 408 gps.longitude = 0;
RCMISbed 0:07e03c88e90b 409 gps.x = 0;
RCMISbed 0:07e03c88e90b 410 gps.y = 0;
RCMISbed 0:07e03c88e90b 411 gps.z = 0;
RCMISbed 0:07e03c88e90b 412 gps.speedmin = 5;
RCMISbed 0:07e03c88e90b 413 gps.Status = 0;
RCMISbed 0:07e03c88e90b 414 // USB.printf("."); wait(0.3);
RCMISbed 0:07e03c88e90b 415
RCMISbed 0:07e03c88e90b 416 // Initialisation/ Type/ objet IMU
RCMISbed 0:07e03c88e90b 417 imu.fValid = true;
RCMISbed 0:07e03c88e90b 418 imu.fFailed = false;
RCMISbed 0:07e03c88e90b 419 imu.Status = 0;
RCMISbed 0:07e03c88e90b 420 imu.roll = 0;
RCMISbed 0:07e03c88e90b 421 imu.pitch = 0;
RCMISbed 0:07e03c88e90b 422 imu.yaw = 0;
RCMISbed 0:07e03c88e90b 423 imu.q0 = 1;
RCMISbed 0:07e03c88e90b 424 imu.q1 = 0;
RCMISbed 0:07e03c88e90b 425 imu.q2 = 0;
RCMISbed 0:07e03c88e90b 426 imu.q3 = 0;
RCMISbed 0:07e03c88e90b 427 imu.liax = 0;
RCMISbed 0:07e03c88e90b 428 imu.liay = 0;
RCMISbed 0:07e03c88e90b 429 imu.liaz = 0;
RCMISbed 0:07e03c88e90b 430 imu.grvx = 0;
RCMISbed 0:07e03c88e90b 431 imu.grvy = 0;
RCMISbed 0:07e03c88e90b 432 imu.grvz = 0;
RCMISbed 0:07e03c88e90b 433 imu.gyrox = 0;
RCMISbed 0:07e03c88e90b 434 imu.gyroy = 0;
RCMISbed 0:07e03c88e90b 435 imu.gyroz = 0;
RCMISbed 0:07e03c88e90b 436 imu.magx = 0;
RCMISbed 0:07e03c88e90b 437 imu.magy = 0;
RCMISbed 0:07e03c88e90b 438 imu.pressure = 0;
RCMISbed 0:07e03c88e90b 439 imu.humidity = 0;
RCMISbed 0:07e03c88e90b 440 imu.temperature = 0;
RCMISbed 0:07e03c88e90b 441 imu.altitude = 0;
RCMISbed 0:07e03c88e90b 442 imu.magz = 0;
RCMISbed 0:07e03c88e90b 443 imu.Status = 0;
RCMISbed 0:07e03c88e90b 444 if(imu.fValid== true){
RCMISbed 0:07e03c88e90b 445 attitude.reset(); wait(1);
RCMISbed 0:07e03c88e90b 446 if (!attitude.check()){ // Presence Capteur Attitude ?
RCMISbed 0:07e03c88e90b 447 while (1){l1 =!l1; l2 =!l2; l3 =!l3; l4 =!l4; son= !son; wait(0.3);};
RCMISbed 0:07e03c88e90b 448 }; // Arret et flash LED et SOUND si le capteur d'attitude n'est pas reconnu
RCMISbed 0:07e03c88e90b 449 // attitude.setpowermede(); // A definir
RCMISbed 0:07e03c88e90b 450 attitude.set_accel_units(MILLIG); // Unite Accelleration MILLIG ou MPERSPERS
RCMISbed 0:07e03c88e90b 451 attitude.set_anglerate_units(DEG_PER_SEC); // Unite Vitesse Angulaire RAD_PER_SEC ou DEG_PER_SEC
RCMISbed 0:07e03c88e90b 452 attitude.set_angle_units(RADIANS); // Unite Angle RADIANS ou DEGREES
RCMISbed 0:07e03c88e90b 453 attitude.set_temp_units(CENTIGRADE); // Unite Temperature CENTIGRADE ou FAHRENHEIT
RCMISbed 0:07e03c88e90b 454 attitude.setmode(OPERATION_MODE_NDOF); // Mode Fonctionnement
RCMISbed 0:07e03c88e90b 455
RCMISbed 0:07e03c88e90b 456 // if (pression.init() != 0) { // Presence Capteur pression atmosphérique ?
RCMISbed 0:07e03c88e90b 457 // while (1){L1 =!L1; L2 =!L2; L3 =!L3; L4 =!L4; SOUND= !SOUND; wait(0.3);};
RCMISbed 0:07e03c88e90b 458 // };
RCMISbed 0:07e03c88e90b 459 // Arret et flash LED et SOUND si le capteur de pression n'est pas reconnu
RCMISbed 0:07e03c88e90b 460 }; // fin if(imu.fvalid== true){
RCMISbed 0:07e03c88e90b 461 // USB.printf("."); wait(0.3);
RCMISbed 0:07e03c88e90b 462
RCMISbed 0:07e03c88e90b 463
RCMISbed 0:07e03c88e90b 464 // Initialisation/ Objet/ MATRICE
RCMISbed 0:07e03c88e90b 465 int cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 466 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 467 while(cntr< matrixSMAX){while(cntc< matrixSMAX){A(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 468 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 469 while(cntr< matrixSMAX){while(cntc< matrixSMAX){B(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 470 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 471 while(cntr< matrixSMAX){while(cntc< matrixSMAX){H(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 472 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 473 while(cntr< matrixSMAX){while(cntc< matrixSMAX){Q(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 474 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 475 while(cntr< matrixSMAX){while(cntc< matrixSMAX){R(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 476 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 477 while(cntr< matrixSMAX){while(cntc< 1){pXk(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 478 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 479 while(cntr< matrixSMAX){while(cntc< matrixSMAX){pPk(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 480 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 481 while(cntr< matrixSMAX){while(cntc< 1){Xk(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 482 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 483 while(cntr< matrixSMAX){while(cntc< matrixSMAX){Pk(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 484 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 485 while(cntr< matrixSMAX){while(cntc< matrixSMAX){Kk(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 486 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 487 while(cntr< matrixSMAX){while(cntc< 1){Zk(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 488 cntr=0, cntc= 0;
RCMISbed 0:07e03c88e90b 489 while(cntr< matrixSMAX){while(cntc< 1){Uk(cntr,cntc)= 0;++cntc;};++cntr;};
RCMISbed 0:07e03c88e90b 490
RCMISbed 0:07e03c88e90b 491 // Initialisation/ Objet/ Matrice Etat/ A
RCMISbed 0:07e03c88e90b 492 A<< 1, TSample, 0, 0,
RCMISbed 0:07e03c88e90b 493 0, 1 , 0, 0,
RCMISbed 0:07e03c88e90b 494 0, 0 , 0, TSample,
RCMISbed 0:07e03c88e90b 495 0, 0 , 0, 1;
RCMISbed 0:07e03c88e90b 496
RCMISbed 0:07e03c88e90b 497 // Initialisation/ Objet/ Matrice Commande/ B
RCMISbed 0:07e03c88e90b 498 B<< 1, 0, 0, 0,
RCMISbed 0:07e03c88e90b 499 0, 1, 0, 0,
RCMISbed 0:07e03c88e90b 500 0, 0, 1, 0,
RCMISbed 0:07e03c88e90b 501 0, 0, 0, 1;
RCMISbed 0:07e03c88e90b 502
RCMISbed 0:07e03c88e90b 503 // Initialisation/ Objet/ Matrice Observabilite (Mesure)/ H
RCMISbed 0:07e03c88e90b 504 H<< 1, 1, 0, 0,
RCMISbed 0:07e03c88e90b 505 0, 1, 0, 0,
RCMISbed 0:07e03c88e90b 506 0, 0, 1, 0,
RCMISbed 0:07e03c88e90b 507 0, 0, 0, 1;
RCMISbed 0:07e03c88e90b 508
RCMISbed 0:07e03c88e90b 509 // Initialisation/ Objet/ Matrice Covariance Etat/ P
RCMISbed 0:07e03c88e90b 510 Pk<< 1, 0, 0, 0,
RCMISbed 0:07e03c88e90b 511 0, 1, 0, 0,
RCMISbed 0:07e03c88e90b 512 0, 0, 1, 0,
RCMISbed 0:07e03c88e90b 513 0, 0, 0, 1;
RCMISbed 0:07e03c88e90b 514
RCMISbed 0:07e03c88e90b 515 // Initialisation/ Objet/ Matrice Covariance Bruit Etat/ Q
RCMISbed 0:07e03c88e90b 516 Q<< 0, 0, 0, 0,
RCMISbed 0:07e03c88e90b 517 0, 0, 0, 0,
RCMISbed 0:07e03c88e90b 518 0, 0, 0, 0,
RCMISbed 0:07e03c88e90b 519 0, 0, 0, 0;
RCMISbed 0:07e03c88e90b 520
RCMISbed 0:07e03c88e90b 521 // Initialisation/ Objet/ Matrice Covariance Bruit Observabilité (Mesure)/ R
RCMISbed 0:07e03c88e90b 522 R<< 0.2, 0, 0, 0,
RCMISbed 0:07e03c88e90b 523 0, 0.2, 0, 0,
RCMISbed 0:07e03c88e90b 524 0, 0, 0.2, 0,
RCMISbed 0:07e03c88e90b 525 0, 0, 0, 0.2;
RCMISbed 0:07e03c88e90b 526
RCMISbed 0:07e03c88e90b 527 // Initialisation/ Objet/ Matrice Identité/ I
RCMISbed 0:07e03c88e90b 528 I<< 1, 0, 0, 0,
RCMISbed 0:07e03c88e90b 529 0, 1, 0, 0,
RCMISbed 0:07e03c88e90b 530 0, 0, 1, 0,
RCMISbed 0:07e03c88e90b 531 0, 0, 0, 1;
RCMISbed 0:07e03c88e90b 532
RCMISbed 0:07e03c88e90b 533 TMainProcess.attach(&MainProcess_Isr, TSample);
RCMISbed 0:07e03c88e90b 534 UART1.attach(&Rx_UART1_Isr, Serial::RxIrq);
RCMISbed 0:07e03c88e90b 535 UART2.attach(&Rx_UART2_Isr, Serial::RxIrq);
RCMISbed 0:07e03c88e90b 536 USB.attach(&Rx_USB_Isr, Serial::RxIrq);
RCMISbed 0:07e03c88e90b 537 son= 0;
RCMISbed 0:07e03c88e90b 538 l1= OFF; l2= OFF; l3= OFF; l4= OFF;
RCMISbed 0:07e03c88e90b 539 USB.printf("%s/ Essai ...\r\n",produit); wait(3);
RCMISbed 0:07e03c88e90b 540 return status;
RCMISbed 0:07e03c88e90b 541 }
RCMISbed 0:07e03c88e90b 542
RCMISbed 0:07e03c88e90b 543
RCMISbed 0:07e03c88e90b 544
RCMISbed 0:07e03c88e90b 545
RCMISbed 0:07e03c88e90b 546
RCMISbed 0:07e03c88e90b 547
RCMISbed 0:07e03c88e90b 548
RCMISbed 0:07e03c88e90b 549 //Definition/ Procedures/ Procedure Principale
RCMISbed 0:07e03c88e90b 550 //==============================================================================
RCMISbed 0:07e03c88e90b 551 int main() {
RCMISbed 0:07e03c88e90b 552 int status= 0;
RCMISbed 0:07e03c88e90b 553 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);};
RCMISbed 0:07e03c88e90b 554 while(1){};
RCMISbed 0:07e03c88e90b 555 return status;
RCMISbed 0:07e03c88e90b 556 }