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