Essai GPS Fusion
Dependencies: BME280 BNO055 Eigen ds3231 mbed
objet.h
- Committer:
- RCMISbed
- Date:
- 2018-10-22
- Revision:
- 1:03547f392fb7
- Parent:
- 0:07e03c88e90b
File content as of revision 1:03547f392fb7:
#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;