richard misi
/
LPC1768-GPS-FUSION-17102018-1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: BME280 BNO055 Eigen ds3231 mbed
Embed:
(wiki syntax)
Show/hide line numbers
objet.h
00001 00002 #include <string> 00003 00004 //1- Definition/ Constantes 00005 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00006 00007 #define wBUFFER 300 // Processus principal/ Trame/ Profondeur MAX du buffer de donnees (exprimé en caracteres) 00008 #define wDTRAME 6 // Processus principal/ Trame/ Nombre de donnees MAX par trame 00009 00010 00011 // Definition/ Constante/ Process General/ Nombre d'objet Event MAX 00012 #define nbEVENTMAX 15 00013 00014 // Definition/ Constante/ Nombre de LED intégrées MAX 00015 #define nbLEDMAX 4 // Carte/ nombre Max LED 00016 00017 // Definition/ Constante/ Nombre de Moteurs MAX 00018 #define nbMOTORMAX 2 00019 00020 // Definition/ Constante/ Nombre de SERVO MAX 00021 #define nbSERVOMAX 2 00022 00023 //Definition/ Parametrage/ GPS/ Parcours/ Nombre maximal de WP 00024 #define nbWPMAX 100 // Parcours GPS/ Nombre de Way Point Maximum enregistrés 00025 00026 //Definition/ Constante/ Nombre d'objet PID MAX 00027 #define nbPIDMAX 2 00028 00029 //Definition/ Constante/ Nombre d'objet commande.axe 00030 #define nbAXEMAX 2 00031 00032 //Definition/ Constante/ ojet PAD/ nb axe MAX et nb SW MAX 00033 #define padNBAXEMAX 2 00034 #define padNBSWMAX 4 00035 00036 //Definition/ Constante/ Nombre Parametres MAX 00037 #define nbPARAMAX 11 00038 00039 00040 // Definition/ Protocole/ Constantes/ Mode 00041 #define mSTB 0 // Mode/ STANDBY 00042 #define mSTP 1 // Mode/ STOP 00043 #define mMAN 2 // Mode/ MANUAL 00044 #define mPA 4 // Mode/ Pilote Automatique 00045 #define mWP 8 // Mode/ Enregistrement WP 00046 #define mRAZ 64 // Mode/ Remise à 0 des commandes 00047 #define mCHG 128 // Mode/ recharge Accumulateurs 00048 00049 00050 //2- Definition/ Type de donnees 00051 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00052 00053 //2.1- Definition/ Type/ global/ data/ objet GLOBAL 00054 typedef struct{ 00055 bool fValid; // Validation module 00056 string designation; // Identification produit 00057 string version; // Version produit (v.xx/yy , xx: annee, yy: semaine dans l'annee) 00058 bool debug; // Activation du mode DEBUG 00059 int nbLED; // Nombre LED integrées 00060 int tBATT; // Batterie/ Structure Chimique 00061 int NbElement; // Batterie/ Nombre Elements 00062 int Dynmin; // Variables/ dynamique minimum 00063 int DynMAX; // Variables/ dynamique MAXIMUM 00064 float TSample; // Periode d'echantillonnage du processus (exprimée en secondes) 00065 int TPWM; // periode PWM (exprimée en micro secondes) 00066 int USBBaud; // UART sur USB BaudRate 00067 int UART1Baud; // UART 1 BaudRate 00068 int UART2Baud; // UART 2 BaudRate 00069 int I2CBaud; // Frequence bus I2C 00070 int CANBaud; // Frequence bus CAN 00071 struct{ // Event 00072 bool fValid; // Event/ Validité 00073 bool fPeriode; // Event/ Periodicité 00074 int wdt; // Event/ WatchDog 00075 int wdtMAX; // Event/ WatchDog MAX 00076 }event[nbEVENTMAX]; 00077 } GLOBAL_t; 00078 00079 00080 //2.2- Definition/ Type/ objet TRAME 00081 typedef struct{ 00082 bool fValid; // Validation module 00083 int wdtTX; // WatchDog Emission 00084 int wdtRX; // WatchDog Reception 00085 int pcar; // Pointeur de caracteres 00086 char sb; // Protocole communication/ Start Beacon: 00087 char dlb; // Protocole communication/ Delimiter Beacon 00088 char eb; // Protocole communication/ End Beacon 00089 int id; // Mot identificateur de trame 00090 char rx[wBUFFER]; // tableau des caracteres recus 00091 char tx[wBUFFER]; // tableau des caracteres envoyés 00092 bool fSB, fEB; // status startB et endB 00093 int c[wDTRAME]; // Data Consigne 00094 int r[wDTRAME]; // Data Reponse 00095 int nbdata; // Nombre de Data sur la Trame 00096 } TRAME_t; 00097 00098 00099 00100 //2.3- Definition/ Type/ objet COMMANDE 00101 typedef struct{ 00102 struct{ 00103 bool fValid; // Validation Module 00104 float in; // Variable Entree 00105 float out; // Variable Sortie 00106 float dzm; // Valeur Dead Zone inferieure (0..1) 00107 float dzM; // Valeur Dead Zone supérieure (0..1) 00108 float cti; // Valeur Constante de temps Integrale (0..1) 00109 float ctf; // Valeur Constante de temps Proportionnelle (0..1) 00110 float ctg; // Valeur Constante de Temps Filtre de sortie 00111 float cgg; // Valeur Gain Filtre de sortie 00112 float l; // Valeur calcul loi non lineaire 00113 float i, i0; // Valeur calcul integral 00114 float p; // Valeur calcul proportionnelle 00115 float c; // Valeur calcul proportionnelle - integrale 00116 float x, dx; // Valeur filtre passe bas 00117 }axe[nbAXEMAX]; 00118 } COMMANDE_t; 00119 00120 00121 00122 00123 00124 //2.4- Definition/ Type/ objet MUX (multiplexeur) 00125 typedef struct{ 00126 bool fValid; // Validation Module 00127 float in0x, in0y, in1x, in1y, in2x, in2y, // Variables d'état d'entree du module: variables d'entree 00128 outx, outy; // Variables d'état de sortie du module: variables de sortie 00129 unsigned char sel; // Variables d'état d'entree du module: selection de la variable d'entrée 00130 unsigned char Status; // Variables Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7: 00131 }MUX_t; 00132 00133 00134 00135 //2.5- Definition/ Type/ objet ENERGIE 00136 typedef struct{ 00137 bool fValid, fCHARGE, fVhigh, fVlow, fIhigh, fIlow, fFailed; 00138 unsigned char tBATT; // Type de batterie (1: LEAD, 2: NIMH, 2: LION, 3: LIPO) // Definition/ Parametres/ Type de batterie 00139 unsigned char NbElement; // Nombre Element en Serie 00140 int seqPointer; // Sequenceur du processus de charge 00141 float tension, courant; // Variables differentielles Energie, Tension, Courant Acquisition 00142 float dE,dV,dI; // Variables differentielles Energie, Tension, Courant Filtrage 00143 float E, E0, V, I; // Variables Energie, Tension, Courant 00144 float VMax, Vmin, IMax, Imin; // Parametres internes 00145 int wPWM, pPWM; // Largeur Impulsion, Periode PWM (chargeur) 00146 unsigned char Status; // Variables Status du module s0: fValid, s1: fCharge, s2:fVlow, s3:fVhigh, s4:fIlow, s5:fIhigh, s6: NA, s7: fFailed 00147 } ENERGY_t; 00148 00149 00150 //2.6- Definition/ Type/ objet GPS 00151 typedef struct{ 00152 int wdtRX; // WatchDog reception 00153 bool fValid; // Validité Module GPS 00154 int baud; // BaudRate 00155 int TSample; // Periode rafraichissement des trames 00156 double utc; // Temps universel 00157 float vground; // Vitesse Sol 00158 float heading; // Cap vrai 00159 int fix; // Fix qualification : (0 = non valide, 1 = Fix GPS, 2 = Fix DGPS) 00160 int nbsat; // Nombre de Sattelites 00161 float dop; // Dilution horizontale 00162 double latitude, longitude, altitude; // Coordonnées NED/ Latitude, Longitude, Altitude 00163 double x, y, z; // Coordonnées ECEF/ x, y, z 00164 int speedmin; 00165 unsigned char Status; //Variables Status du module s0:fValid, s1:fLocked, s2:na, s3:na, s4:na, s5:na, s6:na, s7:fFailed 00166 } GPS_t; 00167 00168 00169 //2.7- Definition/ Type/ objet IMU 00170 typedef struct{ 00171 bool fValid, fFailed; // Validation module 00172 float roll, pitch, yaw; // Variables attitude; Roulis, Tangage, Lacet 00173 float q0, q1, q2, q3; // Variables attitude/ Quarternions 00174 float liax, liay, liaz; // Variables acceleration lineaire x, y, z 00175 float grvx, grvy, grvz; // Variables gravité 00176 float gyrox, gyroy, gyroz; // Variables gyroscopiques x, y, z 00177 float magx, magy, magz; // Variables magnetometriques x, y, z 00178 float pressure; // Variable pression atmospherique 00179 float temperature; // Variable temperature atmosphérique 00180 float altitude; // Variables altimétrique (capteur pression atmosphérique) 00181 float humidity; // Variable taux humidite atmosphérique (capteur pression atmosphérique) 00182 unsigned char Status; // Variables Status module. s0:fValid, s1:fLocked, s2:na, s3:na, s4:na, s5:na, s6:na, s7:fFailed 00183 } IMU_t; 00184 00185 00186 //2.8- Definition/ Type/ objet PA (pilote automatique) 00187 typedef struct{ 00188 bool fValid, fGPSLocked, fWPLocked, fDmin, fDMax, fFailed; 00189 double wLNG, wLAT, // Variables d'état d'entree du module: Coordonnnees Consigne (WayPoint à rejoindre) 00190 mLNG, mLAT; // Variables d'état d'entree du module: Coordonnees reponse (mesure GPS) 00191 float mSPD, // Variables d'état d'entree du module: Vitesse (km/h) (mesure GPS) 00192 mHDG, // Variables d'état d'entree du module: Cap (deg) (mesure GPS) 00193 CtG, // Cap à suivre au WP (calculé) 00194 DtG, // Distance à parcourir au WP (calculée) 00195 cPowerMax, // Niveau de puissance Max (0 .. 1) 00196 Dmin, DMax, // Distance Max et minau WP 00197 Kp, // Correction/ Gain Proportionnel (exprimé en % 0..300) 00198 outx, outy; // Variables d'état de sortie du module 00199 unsigned char Status; // Variables Status du module s0: fValid, s1: fGPSLocked, s2: fWPLocked, s3:fDmin, s4:fDMax, s5: NA, s6: NA,s7: fFailed 00200 } PA_t; 00201 00202 /* 00203 00204 //2.9- Definition/ Type/ objet DIGITAL (Accessoire) 00205 typedef struct{ 00206 bool fValid; // Validation Module 00207 unsigned int cDigital; // Variables d'état d'entree du module (c0: Klaxon, c1: Phare) 00208 unsigned int rDigital; // Variables d'état de sortie du module (c0: Klaxon, c1: Phare) 00209 unsigned char Status; // Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7: 00210 }DIGITAL_t; 00211 00212 00213 //2.10- Definition/ Type/ objet SERVO 00214 typedef struct{ 00215 bool fValid; // Validation Module 00216 int c; // Variables d'état d'entree du module/ Variable (0 .. 1) 00217 float x, dx, tSERVO; // Variables d'état Filtre x, dx/ constante de temps du filtre 00218 int r; // Variables d'état de sortie du module/ Variable (0 .. 1) 00219 int pmin; // Constante/ valeur position minimum 00220 int pmax; // Constante/ valeur position maximum 00221 unsigned char Status; // Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7: 00222 } SERVO_t; 00223 */ 00224 00225 //2.11- Definition/ Type/ objet LOI 00226 typedef struct{ 00227 bool fValid; // Validation Module 00228 float inx; // Variable/ Entree/ Power 00229 float iny; // Variable/ Entree/ Teta 00230 float outx; // Variable/ Sortie/ Gauche 00231 float outy; // Variable/ Sortie/ Droit 00232 unsigned char Status; // Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7: 00233 } LAW_t; 00234 00235 00236 00237 //2.12- Definition/ Type/ objet filtre PID 00238 typedef struct{ 00239 bool fValid; // Filtre PID/ Validite 00240 float inc; // Filtre PID/ Variable/ Consigne 00241 float inr; // Filtre PID/ Variable/ FeedBack 00242 float eM; // Filtre PID/ Variable/ Erreur (Consigne- Feedback) 00243 float eMp; 00244 float eMd; 00245 float eMd0; 00246 float uMi; 00247 float uMi0; 00248 float uMd; 00249 float uMd0; 00250 float out; // Filtre PID/ Variable/ Sortie 00251 float cPID1; // Filtre PID/ Constante/1/ 00252 float cPID2; // Filtre PID/ Constante/2/ 00253 float cPID3; // Filtre PID/ Constante/3/ 00254 float cPID4; // Filtre PID/ Constante/4/ 00255 float cPID5; // Filtre PID/ Constante/5/ 00256 float cPID6; // Filtre PID/ Constante/6/ 00257 unsigned char Status; // Filtre PID/ Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7: 00258 } PID_t; 00259 00260 00261 //2.13- Definition/ Type/ objet MOTEUR 00262 typedef struct{ 00263 bool fValid; // Validite Module 00264 float in; // Variable d'état/ entrée/ puissance (0..1) 00265 float dx, x; // Variables d'etat/ interne / Filtre Passe Bas 00266 float out; // Variables d'état/ sortie / puissance (0.. 1) 00267 float tMOTOR; // Constante de temps/ Filtre Passe Bas 00268 float gMOTOR; // Constante/ Gain/ Filtre Passe Bas 00269 int pmin; // Constante/ valeur minimum 00270 int pmax; // Constante/ valeur maximum 00271 unsigned char Status; // Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7: 00272 } MOTOR_t; 00273 00274 //2.14- Definition/ Type/ objet DIGITAL 00275 typedef struct{ 00276 bool fValid; // Validite Module 00277 int in; // Variable d'état/ entrée 00278 int out; // Variables d'état/ sortie 00279 unsigned char Status; // Variable Status du module s0:, s1:, s2:, s3:, s4:, s5:, s6:, s7: 00280 } DIGITAL_t; 00281 00282 //2.14- Definition/ Type/ objet TRACK 00283 typedef struct{ 00284 bool fValid; // TRACK/ validite/ module 00285 bool fRecord; // TRACK/ Acquisition/ En cours 00286 int nbWP; // TRACK/ nombre WP 00287 int pWP; // TRACK/ pointeur WP 00288 struct{ // TRACK/ structure WP 00289 bool fValid; // wp/ valide 00290 double lat, lng; // wp/ Coordonnees Geographiques 00291 }wp[nbWPMAX]; 00292 int Status; // wp/ status 00293 } TRACK_t; 00294 00295 00296 00297 //2.15- Definition/ Type/ objet MOBILE 00298 typedef struct{ 00299 bool fValid; // Mobile/ Validite 00300 int cWDTRX; // Mobile/ Contante/ WatchDog RX 00301 float inx, iny, outx, outy; // Mobile/ Commande: Niveau de puissance vectorielle: Amplidude/ Phase exprimé de 0 à 100% 00302 int cMODE, rMODE; // Mobile/ Mode de Fonctionnement 00303 00304 // double lng, lat, speed, head; // Mobile/ Variables localisation (GPS) 00305 00306 float A[6][6]; 00307 00308 float Px, Py, Pz; // Mobile/ Variables / Position Spatiale 00309 float Vx, Vy, Vz; // Mobile/ Variables / Vitesse Spatiale 00310 float Ax, Ay, Az; // Mobile/ Variables / Acceleration Spatiale 00311 float Heading; // Mobile/ Variable / Cap 00312 float roll, pitch, yaw; // Mobile/ Variables attitude (IMU) 00313 float temperature; // Mobile/ Variable temperature atmosphérique 00314 float pressure; // Mobile/ Variable pression atmosphérique 00315 float humidity; // Mobile/ Variable humidite atmosphérique 00316 float altitude; // Mobile/ Variable altimétrique 00317 int Status; // Mobile/ Status Module 00318 } MOBILE_t; 00319 00320 00321 //2.16- Definition/ objet PAD 00322 typedef struct{ 00323 bool fValid; 00324 struct{ 00325 float in; // Variable Entree 00326 float out; // Variable Sortie 00327 int scB; // Pourcentage Proportionnal- Integral (0..100) sc=0: Commande Integrale pure .... sc= 100: Commande Proportionnelle pure 00328 int dzB; // Pourcentage Dead Zone (0..100) 00329 int znlB; // Pourcentage Zone Non Lineaire (0..100) 00330 float ctiB; // Pourcentage Constante de temps loi integrale (0..100) 00331 float gainI; // Gain Integrale deuxieme zone lineaire 00332 float ctfB; // Pourcentage Constante de temps loi proportionnelle (0..100) 00333 float sc; // Valeur Proportionnel Integral 00334 float dzm; // Valeur Dead Zone inferieure 00335 float dzM; // Valeur Dead Zone supérieure 00336 float znlm; // Valeur Non Linearite inferieure 00337 float znlM; // Valeur Non Linearite supérieure 00338 float cti; // Valeur Constante de temps Integrale 00339 float ctf; // Valeur Constante de temps Proportionnelle 00340 float l; // Valeur calcul loi non lineaire 00341 float i, i0; // Valeur calcul integral 00342 float p; // Valeur calcul proportionnelle 00343 // float fm[5]; // Valeur filtre moyenneur 00344 float x, dX, X; // Valeur filtre passe bas 00345 }axe[padNBAXEMAX]; 00346 struct{ 00347 bool value; 00348 }sw[padNBSWMAX]; 00349 00350 // float roulis, tangage; 00351 } PAD_t; 00352 00353 00354 //2.17- Definition/ Type/ objet CALENDRIER 00355 typedef struct{ 00356 bool fValid; // Validation module 00357 int wdtTX; // WatchDog Emission 00358 long int epoque; // Nombre de secondes depuis le 01/ 01/ 1970 (UNIX Time) 00359 int seconde; // Seconde 00360 int minute; // Minute 00361 int heure; // Heure 00362 int jour; // Jour 00363 int mois; // Mois 00364 int annee; // Annee 00365 } CALENDAR_t; 00366 00367 00368 00369 //2.18- Definition/ Type/ Coordonnées géographiques NED (North - East - Down)/ Référentiel WGS-84 00370 typedef struct{ 00371 double Lat; // Latitude radians, + = North 00372 double Lon; // Longitude radians, + = East 00373 double Alt; // Altitude métres au dessus de l'elipsoide de référence 00374 } NED_t; 00375 00376 00377 00378 //2.19- Definition/ Type/ Coordonnées géographiques ECEF (Earth-Centered, Earth-Fixed)/ Référentiel WGS-84 00379 typedef struct{ 00380 double X; // Valeur em mètre 00381 double Y; // Valeur em mètre 00382 double Z; // Valeur em mètre 00383 } ECEF_t; 00384 00385 00386 00387 00388 00389 00390 00391 00392 00393
Generated on Wed Jul 20 2022 01:53:01 by
1.7.2