richard misi
Essai GPS Fusion
Dependencies: BME280 BNO055 Eigen ds3231 mbed
Revision 0:07e03c88e90b, committed 2018-10-17
- Comitter:
- RCMISbed
- Date:
- Wed Oct 17 11:39:38 2018 +0000
- Child:
- 1:03547f392fb7
- Commit message:
- gps fusion;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BME280.lib Wed Oct 17 11:39:38 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/jont/code/BME280/#691e06db489f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BNO055.lib Wed Oct 17 11:39:38 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/RCMISbed/code/BNO055/#fe45288757f6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Eigen.lib Wed Oct 17 11:39:38 2018 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/ykuroda/code/Eigen/#13a5d365ba16
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ds3231.lib Wed Oct 17 11:39:38 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/RCMISbed/code/ds3231/#c12eab714b4e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/essai-gps-fusion.cpp Wed Oct 17 11:39:38 2018 +0000
@@ -0,0 +1,556 @@
+/*
+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;
+ }
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Wed Oct 17 11:39:38 2018 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#0fdfcf7350896a9c0b57c4a18237677abfe25f1a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/objet.h Wed Oct 17 11:39:38 2018 +0000
@@ -0,0 +1,393 @@
+
+#include <string>
+
+//1- Definition/ Constantes
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+#define wBUFFER 300 // Processus principal/ Trame/ Profondeur MAX du buffer de donnees (exprimé en caracteres)
+#define wDTRAME 6 // Processus principal/ Trame/ Nombre de donnees MAX par trame
+
+
+// Definition/ Constante/ Process General/ Nombre d'objet Event MAX
+#define nbEVENTMAX 15
+
+// Definition/ Constante/ Nombre de LED intégrées MAX
+#define nbLEDMAX 4 // Carte/ nombre Max LED
+
+// Definition/ Constante/ Nombre de Moteurs MAX
+#define nbMOTORMAX 2
+
+// Definition/ Constante/ Nombre de SERVO MAX
+#define nbSERVOMAX 2
+
+//Definition/ Parametrage/ GPS/ Parcours/ Nombre maximal de WP
+#define nbWPMAX 100 // Parcours GPS/ Nombre de Way Point Maximum enregistrés
+
+//Definition/ Constante/ Nombre d'objet PID MAX
+#define nbPIDMAX 2
+
+//Definition/ Constante/ Nombre d'objet commande.axe
+#define nbAXEMAX 2
+
+//Definition/ Constante/ ojet PAD/ nb axe MAX et nb SW MAX
+#define padNBAXEMAX 2
+#define padNBSWMAX 4
+
+//Definition/ Constante/ Nombre Parametres MAX
+#define nbPARAMAX 11
+
+
+// Definition/ Protocole/ Constantes/ Mode
+#define mSTB 0 // Mode/ STANDBY
+#define mSTP 1 // Mode/ STOP
+#define mMAN 2 // Mode/ MANUAL
+#define mPA 4 // Mode/ Pilote Automatique
+#define mWP 8 // Mode/ Enregistrement WP
+#define mRAZ 64 // Mode/ Remise à 0 des commandes
+#define mCHG 128 // Mode/ recharge Accumulateurs
+
+
+//2- Definition/ Type de donnees
+//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+//2.1- Definition/ Type/ global/ data/ objet GLOBAL
+typedef struct{
+ bool fValid; // Validation module
+ string designation; // Identification produit
+ string version; // Version produit (v.xx/yy , xx: annee, yy: semaine dans l'annee)
+ bool debug; // Activation du mode DEBUG
+ int nbLED; // Nombre LED integrées
+ int tBATT; // Batterie/ Structure Chimique
+ int NbElement; // Batterie/ Nombre Elements
+ int Dynmin; // Variables/ dynamique minimum
+ int DynMAX; // Variables/ dynamique MAXIMUM
+ float TSample; // Periode d'echantillonnage du processus (exprimée en secondes)
+ int TPWM; // periode PWM (exprimée en micro secondes)
+ int USBBaud; // UART sur USB BaudRate
+ int UART1Baud; // UART 1 BaudRate
+ int UART2Baud; // UART 2 BaudRate
+ int I2CBaud; // Frequence bus I2C
+ int CANBaud; // Frequence bus CAN
+ struct{ // Event
+ bool fValid; // Event/ Validité
+ bool fPeriode; // Event/ Periodicité
+ int wdt; // Event/ WatchDog
+ int wdtMAX; // Event/ WatchDog MAX
+ }event[nbEVENTMAX];
+ } GLOBAL_t;
+
+
+//2.2- Definition/ Type/ objet TRAME
+typedef struct{
+ bool fValid; // Validation module
+ int wdtTX; // WatchDog Emission
+ int wdtRX; // WatchDog Reception
+ int pcar; // Pointeur de caracteres
+ char sb; // Protocole communication/ Start Beacon:
+ char dlb; // Protocole communication/ Delimiter Beacon
+ char eb; // Protocole communication/ End Beacon
+ int id; // Mot identificateur de trame
+ char rx[wBUFFER]; // tableau des caracteres recus
+ char tx[wBUFFER]; // tableau des caracteres envoyés
+ bool fSB, fEB; // status startB et endB
+ int c[wDTRAME]; // Data Consigne
+ int r[wDTRAME]; // Data Reponse
+ int nbdata; // Nombre de Data sur la Trame
+ } TRAME_t;
+
+
+
+//2.3- Definition/ Type/ objet COMMANDE
+typedef struct{
+ struct{
+ bool fValid; // Validation Module
+ float in; // Variable Entree
+ float out; // Variable Sortie
+ float dzm; // Valeur Dead Zone inferieure (0..1)
+ float dzM; // Valeur Dead Zone supérieure (0..1)
+ float cti; // Valeur Constante de temps Integrale (0..1)
+ float ctf; // Valeur Constante de temps Proportionnelle (0..1)
+ float ctg; // Valeur Constante de Temps Filtre de sortie
+ float cgg; // Valeur Gain Filtre de sortie
+ float l; // Valeur calcul loi non lineaire
+ float i, i0; // Valeur calcul integral
+ float p; // Valeur calcul proportionnelle
+ float c; // Valeur calcul proportionnelle - integrale
+ float x, dx; // Valeur filtre passe bas
+ }axe[nbAXEMAX];
+ } COMMANDE_t;
+
+
+
+
+
+//2.4- Definition/ Type/ objet MUX (multiplexeur)
+typedef struct{
+ bool fValid; // Validation Module
+ float in0x, in0y, in1x, in1y, in2x, in2y, // Variables d'état d'entree du module: variables d'entree
+ outx, outy; // Variables d'état de sortie du module: variables de sortie
+ unsigned char sel; // Variables d'état d'entree du module: selection de la variable d'entrée
+ unsigned char Status; // Variables Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7:
+ }MUX_t;
+
+
+
+//2.5- Definition/ Type/ objet ENERGIE
+typedef struct{
+ bool fValid, fCHARGE, fVhigh, fVlow, fIhigh, fIlow, fFailed;
+ unsigned char tBATT; // Type de batterie (1: LEAD, 2: NIMH, 2: LION, 3: LIPO) // Definition/ Parametres/ Type de batterie
+ unsigned char NbElement; // Nombre Element en Serie
+ int seqPointer; // Sequenceur du processus de charge
+ float tension, courant; // Variables differentielles Energie, Tension, Courant Acquisition
+ float dE,dV,dI; // Variables differentielles Energie, Tension, Courant Filtrage
+ float E, E0, V, I; // Variables Energie, Tension, Courant
+ float VMax, Vmin, IMax, Imin; // Parametres internes
+ int wPWM, pPWM; // Largeur Impulsion, Periode PWM (chargeur)
+ unsigned char Status; // Variables Status du module s0: fValid, s1: fCharge, s2:fVlow, s3:fVhigh, s4:fIlow, s5:fIhigh, s6: NA, s7: fFailed
+ } ENERGY_t;
+
+
+//2.6- Definition/ Type/ objet GPS
+typedef struct{
+ int wdtRX; // WatchDog reception
+ bool fValid; // Validité Module GPS
+ int baud; // BaudRate
+ int TSample; // Periode rafraichissement des trames
+ double utc; // Temps universel
+ float vground; // Vitesse Sol
+ float heading; // Cap vrai
+ int fix; // Fix qualification : (0 = non valide, 1 = Fix GPS, 2 = Fix DGPS)
+ int nbsat; // Nombre de Sattelites
+ float dop; // Dilution horizontale
+ double latitude, longitude, altitude; // Coordonnées NED/ Latitude, Longitude, Altitude
+ double x, y, z; // Coordonnées ECEF/ x, y, z
+ int speedmin;
+ unsigned char Status; //Variables Status du module s0:fValid, s1:fLocked, s2:na, s3:na, s4:na, s5:na, s6:na, s7:fFailed
+ } GPS_t;
+
+
+//2.7- Definition/ Type/ objet IMU
+typedef struct{
+ bool fValid, fFailed; // Validation module
+ float roll, pitch, yaw; // Variables attitude; Roulis, Tangage, Lacet
+ float q0, q1, q2, q3; // Variables attitude/ Quarternions
+ float liax, liay, liaz; // Variables acceleration lineaire x, y, z
+ float grvx, grvy, grvz; // Variables gravité
+ float gyrox, gyroy, gyroz; // Variables gyroscopiques x, y, z
+ float magx, magy, magz; // Variables magnetometriques x, y, z
+ float pressure; // Variable pression atmospherique
+ float temperature; // Variable temperature atmosphérique
+ float altitude; // Variables altimétrique (capteur pression atmosphérique)
+ float humidity; // Variable taux humidite atmosphérique (capteur pression atmosphérique)
+ unsigned char Status; // Variables Status module. s0:fValid, s1:fLocked, s2:na, s3:na, s4:na, s5:na, s6:na, s7:fFailed
+ } IMU_t;
+
+
+//2.8- Definition/ Type/ objet PA (pilote automatique)
+typedef struct{
+ bool fValid, fGPSLocked, fWPLocked, fDmin, fDMax, fFailed;
+ double wLNG, wLAT, // Variables d'état d'entree du module: Coordonnnees Consigne (WayPoint à rejoindre)
+ mLNG, mLAT; // Variables d'état d'entree du module: Coordonnees reponse (mesure GPS)
+ float mSPD, // Variables d'état d'entree du module: Vitesse (km/h) (mesure GPS)
+ mHDG, // Variables d'état d'entree du module: Cap (deg) (mesure GPS)
+ CtG, // Cap à suivre au WP (calculé)
+ DtG, // Distance à parcourir au WP (calculée)
+ cPowerMax, // Niveau de puissance Max (0 .. 1)
+ Dmin, DMax, // Distance Max et minau WP
+ Kp, // Correction/ Gain Proportionnel (exprimé en % 0..300)
+ outx, outy; // Variables d'état de sortie du module
+ unsigned char Status; // Variables Status du module s0: fValid, s1: fGPSLocked, s2: fWPLocked, s3:fDmin, s4:fDMax, s5: NA, s6: NA,s7: fFailed
+ } PA_t;
+
+/*
+
+//2.9- Definition/ Type/ objet DIGITAL (Accessoire)
+typedef struct{
+ bool fValid; // Validation Module
+ unsigned int cDigital; // Variables d'état d'entree du module (c0: Klaxon, c1: Phare)
+ unsigned int rDigital; // Variables d'état de sortie du module (c0: Klaxon, c1: Phare)
+ unsigned char Status; // Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7:
+ }DIGITAL_t;
+
+
+//2.10- Definition/ Type/ objet SERVO
+typedef struct{
+ bool fValid; // Validation Module
+ int c; // Variables d'état d'entree du module/ Variable (0 .. 1)
+ float x, dx, tSERVO; // Variables d'état Filtre x, dx/ constante de temps du filtre
+ int r; // Variables d'état de sortie du module/ Variable (0 .. 1)
+ int pmin; // Constante/ valeur position minimum
+ int pmax; // Constante/ valeur position maximum
+ unsigned char Status; // Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7:
+ } SERVO_t;
+*/
+
+//2.11- Definition/ Type/ objet LOI
+ typedef struct{
+ bool fValid; // Validation Module
+ float inx; // Variable/ Entree/ Power
+ float iny; // Variable/ Entree/ Teta
+ float outx; // Variable/ Sortie/ Gauche
+ float outy; // Variable/ Sortie/ Droit
+ unsigned char Status; // Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7:
+ } LAW_t;
+
+
+
+//2.12- Definition/ Type/ objet filtre PID
+typedef struct{
+ bool fValid; // Filtre PID/ Validite
+ float inc; // Filtre PID/ Variable/ Consigne
+ float inr; // Filtre PID/ Variable/ FeedBack
+ float eM; // Filtre PID/ Variable/ Erreur (Consigne- Feedback)
+ float eMp;
+ float eMd;
+ float eMd0;
+ float uMi;
+ float uMi0;
+ float uMd;
+ float uMd0;
+ float out; // Filtre PID/ Variable/ Sortie
+ float cPID1; // Filtre PID/ Constante/1/
+ float cPID2; // Filtre PID/ Constante/2/
+ float cPID3; // Filtre PID/ Constante/3/
+ float cPID4; // Filtre PID/ Constante/4/
+ float cPID5; // Filtre PID/ Constante/5/
+ float cPID6; // Filtre PID/ Constante/6/
+ unsigned char Status; // Filtre PID/ Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7:
+ } PID_t;
+
+
+//2.13- Definition/ Type/ objet MOTEUR
+typedef struct{
+ bool fValid; // Validite Module
+ float in; // Variable d'état/ entrée/ puissance (0..1)
+ float dx, x; // Variables d'etat/ interne / Filtre Passe Bas
+ float out; // Variables d'état/ sortie / puissance (0.. 1)
+ float tMOTOR; // Constante de temps/ Filtre Passe Bas
+ float gMOTOR; // Constante/ Gain/ Filtre Passe Bas
+ int pmin; // Constante/ valeur minimum
+ int pmax; // Constante/ valeur maximum
+ unsigned char Status; // Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7:
+ } MOTOR_t;
+
+//2.14- Definition/ Type/ objet DIGITAL
+typedef struct{
+ bool fValid; // Validite Module
+ int in; // Variable d'état/ entrée
+ int out; // Variables d'état/ sortie
+ unsigned char Status; // Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7:
+ } DIGITAL_t;
+
+//2.14- Definition/ Type/ objet TRACK
+typedef struct{
+ bool fValid; // TRACK/ validite/ module
+ bool fRecord; // TRACK/ Acquisition/ En cours
+ int nbWP; // TRACK/ nombre WP
+ int pWP; // TRACK/ pointeur WP
+ struct{ // TRACK/ structure WP
+ bool fValid; // wp/ valide
+ double lat, lng; // wp/ Coordonnees Geographiques
+ }wp[nbWPMAX];
+ int Status; // wp/ status
+ } TRACK_t;
+
+
+
+//2.15- Definition/ Type/ objet MOBILE
+typedef struct{
+ bool fValid; // Mobile/ Validite
+ int cWDTRX; // Mobile/ Contante/ WatchDog RX
+ float inx, iny, outx, outy; // Mobile/ Commande: Niveau de puissance vectorielle: Amplidude/ Phase exprimé de 0 à 100%
+ int cMODE, rMODE; // Mobile/ Mode de Fonctionnement
+
+ // double lng, lat, speed, head; // Mobile/ Variables localisation (GPS)
+
+ float A[6][6];
+
+ float Px, Py, Pz; // Mobile/ Variables / Position Spatiale
+ float Vx, Vy, Vz; // Mobile/ Variables / Vitesse Spatiale
+ float Ax, Ay, Az; // Mobile/ Variables / Acceleration Spatiale
+ float Heading; // Mobile/ Variable / Cap
+ float roll, pitch, yaw; // Mobile/ Variables attitude (IMU)
+ float temperature; // Mobile/ Variable temperature atmosphérique
+ float pressure; // Mobile/ Variable pression atmosphérique
+ float humidity; // Mobile/ Variable humidite atmosphérique
+ float altitude; // Mobile/ Variable altimétrique
+ int Status; // Mobile/ Status Module
+ } MOBILE_t;
+
+
+//2.16- Definition/ objet PAD
+typedef struct{
+ bool fValid;
+ struct{
+ float in; // Variable Entree
+ float out; // Variable Sortie
+ int scB; // Pourcentage Proportionnal- Integral (0..100) sc=0: Commande Integrale pure .... sc= 100: Commande Proportionnelle pure
+ int dzB; // Pourcentage Dead Zone (0..100)
+ int znlB; // Pourcentage Zone Non Lineaire (0..100)
+ float ctiB; // Pourcentage Constante de temps loi integrale (0..100)
+ float gainI; // Gain Integrale deuxieme zone lineaire
+ float ctfB; // Pourcentage Constante de temps loi proportionnelle (0..100)
+ float sc; // Valeur Proportionnel Integral
+ float dzm; // Valeur Dead Zone inferieure
+ float dzM; // Valeur Dead Zone supérieure
+ float znlm; // Valeur Non Linearite inferieure
+ float znlM; // Valeur Non Linearite supérieure
+ float cti; // Valeur Constante de temps Integrale
+ float ctf; // Valeur Constante de temps Proportionnelle
+ float l; // Valeur calcul loi non lineaire
+ float i, i0; // Valeur calcul integral
+ float p; // Valeur calcul proportionnelle
+// float fm[5]; // Valeur filtre moyenneur
+ float x, dX, X; // Valeur filtre passe bas
+ }axe[padNBAXEMAX];
+ struct{
+ bool value;
+ }sw[padNBSWMAX];
+
+// float roulis, tangage;
+} PAD_t;
+
+
+//2.17- Definition/ Type/ objet CALENDRIER
+typedef struct{
+ bool fValid; // Validation module
+ int wdtTX; // WatchDog Emission
+ long int epoque; // Nombre de secondes depuis le 01/ 01/ 1970 (UNIX Time)
+ int seconde; // Seconde
+ int minute; // Minute
+ int heure; // Heure
+ int jour; // Jour
+ int mois; // Mois
+ int annee; // Annee
+ } CALENDAR_t;
+
+
+
+//2.18- Definition/ Type/ Coordonnées géographiques NED (North - East - Down)/ Référentiel WGS-84
+typedef struct{
+ double Lat; // Latitude radians, + = North
+ double Lon; // Longitude radians, + = East
+ double Alt; // Altitude métres au dessus de l'elipsoide de référence
+ } NED_t;
+
+
+
+//2.19- Definition/ Type/ Coordonnées géographiques ECEF (Earth-Centered, Earth-Fixed)/ Référentiel WGS-84
+typedef struct{
+ double X; // Valeur em mètre
+ double Y; // Valeur em mètre
+ double Z; // Valeur em mètre
+ } ECEF_t;
+
+
+
+
+
+
+
+
+
+