void TEN_DOF_IUM_Initialise()
{
GPIO_Initialise();
I2C_Initialise();
//Power ON BMP180
I2C_Write8(BMP180_ADDRESS, BMP180_CAL_AC1_LSB, 0x00);
//Power ON L3GD20H
I2C_Write8(L3GD20H_ADDRESS, L3GD20H_REGISTER_CTRL_REG1, L3GD20H_AXIS_AND_POWER_ON);
//Power ON Accelerometer
I2C_Write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x57);
//Power ON Magnetometer
I2C_Write8(LSM303_ADDRESS_MAG,LSM303_REGISTER_MAG_CRA_REG_M,0x00);
//Gain 1.3 NOT SURE WHAT THIS DO.
I2C_Write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, LSM303_MAGGAIN_1_3);
}
int main(int argc, char **argv) {
//
// Machine d'état
//
// int state
// -1: erreur
// 0: non initialisé
// 1: initialisé, pret rotation
// 2: rotation, pret décollage
// 3: décollage
// 4: vol normal
// 5: atterissage
//
state = 0;
printf("\n");
printf("\n================================================");
printf("\n===== XMicroDrone - Controlleur V1.1 =====");
printf("\n================================================");
printf("\n");
//
// Récupérons les numéros de devices séries (paramétre optionnel)
//
int premierDevice = 0; // La valeur par défaut, qu'on utilisera s'il n'y a pas d'arguments
if (argc >= 2) {
int arg = atoi(argv[1]);
if (arg >= 0 && arg <= 2) {
premierDevice = arg;
} else {
printf("Argument devices hors de portée.\n");
}
}
//---------------------------------------------------------------------------
//
//
// INITIALISATION DU PROGRAMME
//
//
//---------------------------------------------------------------------------
//
// Rend le fgets sur stdin non bloquant; FONCTIONNE !
//
int flags = fcntl(0, F_GETFL, 0);
flags |= O_NONBLOCK;
fcntl(0, F_SETFL, flags);
printf("Initialisation des différents modules...");
fflush(stdout);
Misc_Initialise(); // Ne dépend de rien d'autre // Pas de blabla
Maths_Initialise(); // Ne dépend de rien d'autre // Pas de blabla
Params_Initialise(); // Après Maths_Initialise(); (peut utiliser des maths) // Pas de blabla
Asservissement_Initialise(); // Après Params_Initialise();
//Pilote_Initialise(); // Après Params, // Pas de blabla
printf(" OK\n");
printf("Fermeture des serveurs port séries...");
fflush(stdout);
//system("pkill -9 xuartctl"); // kill tous les process utilisant xuartctl TS7500
//usleep(100 * 1000);
printf(" OK... - A priori, A voir si on en a besoin - Pour l'instant ne fait rien\n");
// La fonction suivante émet son propre blabla
I2C_Initialise();
Controlleur_Initialise(0); // Aprés Params_Initialise() et I2C_initialise; // Argument: 1 pour allumer les moteurs successivement
//int grnLedOn = 0;
//int redLedOn = 0;
//Misc_SetRedLed(0); //TS7500
//Misc_SetGrnLed(0);
if (0) {
CommWiFi_Initialise(); // Emet son propre blabla
}
// On se donne temporairement la priorité -15 pour que les xuart l'aient
//
// -15 pour les xuartctl (Port séries) et ts7500ctl (I2C)
// -10 pour nous et ts7500ctl
// -5 pour les trucs systemes importants
// 0 pour le reste
// Plage: -20 (haute priorité) é +20 (faible priorité)
int usePrio = 0;
if (usePrio && setpriority(PRIO_PROCESS, getpid(), -15)) {
printf("ERREUR: setpriority\n");
}
Centrale_Initialise(premierDevice); // Argument: le dev par défaut, -1 pour laisser l'utilisateur choisir
Sonar_Initialise(premierDevice + 1); // Argument: le dev par défaut, -1 pour laisser l'utilisateur choisir
// On se replace a -10
if (usePrio && setpriority(PRIO_PROCESS, getpid(), -10)) {
printf("ERREUR: setpriority !\n");
}
printf("Merci de vérifier les numéros de devices ci-dessus\n");
if (usePrio) {
printf("ET de vous assurer que la priorité du process ts5700ctl est strictement négative !\n");
}
if (0) {
Misc_WaitEnter();
}
float tempsSCum = 0;
int dispType = 1; // ----------------------------------
int attenteSonar = 0;
int attenteCentrale = 0;
int appelsCentrale = 0;
int appelsSonar = 0;
int dernierTestSonar = 0;
int dispUneFois = 0;
int toursAvantVerInputConsole = VERIFICATION_INPUT_CONSOLE_TOUT_LES - 1; // Initialisé ainsi pour vérifier une commande
// dés le 1er tour de boucle
int dispFreqOnce = 0;
//
// Liste des commandes
//
int MAX_COMMAND_LENGTH = 7 + 2; // Prendre la longueur réelle + 2 pour le \n et un cran de sécurité !!!
char chSETPR[] = "set ep"; // Pas d'estimation
//---Réglages des gains
char chSETLP[] = "set lp"; // Régler le gain lacet proportionnel
char chSETLI[] = "set li"; // Régler le gain lacet intégral
char chSETLD[] = "set ld"; // Régler le gain lacet dérivé
char chSETRP[] = "set rp"; // Régler le gain roulis proportionnel
char chSETRI[] = "set ri"; // Régler le gain roulis intégral
char chSETRD[] = "set rd"; // Régler le gain roulis dérivé
char chSETTP[] = "set tp"; // Régler le gain tangage proportionnel
char chSETTI[] = "set ti"; // Régler le gain tangage intégral
char chSETTD[] = "set td"; // Régler le gain tangage dérivé
char chSETAP[] = "set ap"; // Régler le gain altitude proportionnel
char chSETAI[] = "set ai"; // Régler le gain altitude intégral
char chSETAD[] = "set ad"; // Régler le gain altitude dérivé
//videBufferSonar(); TODO Attention ici il faudra décommenter
//---------------------------------------------------------------------------
//
//
// FONCTIONNEMENT NORMAL: BOUCLE INFINIE
//
//
//---------------------------------------------------------------------------
printf("Initialisation terminée !\n");
fflush(stdout);
Misc_ResetElapsedUs();
for (;;) { // La boucle infinie du programme
//
// Vérification de la liaison
//
if (state > 1 && modeI2C > 0) {
attenteSignal++;
if (attenteSignal == 300) { // 5 sec avant warning
printf("\nATTENTION: communication inactive, merci d'appuyer sur entrée !\n");
fflush(stdout);
} else if (attenteSignal == 500) { // 10 sec avant désactivation
if (state == 3 || state == 4) {
state = 5; // Lance un atterissage si en vol
printf("\nERREUR: Aucun signal wifi, atterissage d'urgence !\n");
} else {
state = -1; // Arret d'urgence sinon
printf("\nERREUR: Aucun signal wifi, arret d'urgence !\n");
}
fflush(stdout);
}
}
//////////////////////////////////////////////////////////////////
//
// Regardons si une commande a été tapé dans la console
//
//////////////////////////////////////////////////////////////////
toursAvantVerInputConsole++;
if (toursAvantVerInputConsole >= VERIFICATION_INPUT_CONSOLE_TOUT_LES) {
toursAvantVerInputConsole = 0;
if (1 || Misc_HasData(0)) { // Misc_HasData(0) fonctionne bien en ethernet
char *cptr;
char buffer[MAX_COMMAND_LENGTH]; // Ex 256 au lieu de MAX_COMMAND_LENGTH
cptr = fgets(buffer, MAX_COMMAND_LENGTH, stdin); // Ex 256 au lieu de MAX_COMMAND_LENGTH
if (cptr != NULL) {
attenteSignal = 0; // Watchdog: la communication est active !
//
// Convertissons le char* en char[] pour pouvoir le comparer a l'aide de strncmp
//
char dst[MAX_COMMAND_LENGTH];
strncpy(dst, cptr, MAX_COMMAND_LENGTH - 1);
dst[MAX_COMMAND_LENGTH - 1] = '\0';
if (dispType > 0) {
printf("\n"); // Le dispCommande affiche une ligne puis l'enleve en permanence
}
//
// Comparons le a la liste des commandes implémentés
//
//------------------------------------------------
//--------------- TOUCHE "ENTREE" ----------------
//------------------------------------------------
if ((int)dst[0] == 10) {
dispUneFois = 1;
// Nous poursuivons l'éxécution tranquillement
}
//---------------- Traitons en premier les commandes longues -------------
//------------------------------------------------
//-------------- COMMANDES "SET **" --------------
//------------------------------------------------
else if (strncmp(chSETLP, dst, sizeof chSETLP - 1) == 0) {
GAINS_PRO[0]
= askFloatValue("Gain proportionnel lacet");
} else if (strncmp(chSETLI, dst, sizeof chSETLI - 1) == 0) {
GAINS_INT[0] = askFloatValue("Gain intégral lacet");
} else if (strncmp(chSETLD, dst, sizeof chSETLD - 1) == 0) {
GAINS_DER[0] = askFloatValue("Gain dérivé lacet");
} else if (strncmp(chSETRP, dst, sizeof chSETRP - 1) == 0) {
GAINS_PRO[1]
= askFloatValue("Gain proportionnel roulis");
} else if (strncmp(chSETRI, dst, sizeof chSETRI - 1) == 0) {
GAINS_INT[1] = askFloatValue("Gain intégral roulis");
} else if (strncmp(chSETRD, dst, sizeof chSETRD - 1) == 0) {
GAINS_DER[1] = askFloatValue("Gain dérivé roulis");
} else if (strncmp(chSETTP, dst, sizeof chSETTP - 1) == 0) {
GAINS_PRO[2]
= askFloatValue("Gain proportionnel tangage");
} else if (strncmp(chSETTI, dst, sizeof chSETTI - 1) == 0) {
GAINS_INT[2] = askFloatValue("Gain intégral tangage");
} else if (strncmp(chSETTD, dst, sizeof chSETTD - 1) == 0) {
GAINS_DER[2] = askFloatValue("gain dérivé tangage");
} else if (strncmp(chSETAP, dst, sizeof chSETAP - 1) == 0) {
GAINS_PRO[3]
= askFloatValue("GAIN proportionnel altitude");
} else if (strncmp(chSETAI, dst, sizeof chSETAI - 1) == 0) {
GAINS_INT[3] = askFloatValue("GAIN intégral altitude");
} else if (strncmp(chSETAD, dst, sizeof chSETAD - 1) == 0) {
GAINS_DER[3] = askFloatValue("GAIN dérivé altitude");
}
//------------------------------------------------
//--------------- COMMANDE "SET PR" -----------------
//------------------------------------------------
else if (strncmp(chSETPR, dst, sizeof chSETPR - 1) == 0) {
if (state == 0 || state == 1) {
printf(
"\nTemps d'anticipation actuel de l'estimateur: %i\n",
ESTIMATEUR_TEMPS_DE_REACTION);
int value = askIntValue("TDR");
if (value >= 1 && value <= 5) {
printf("Nouvelle valeur acceptée !");
ESTIMATEUR_TEMPS_DE_REACTION = value;
Asservissement_Initialise();
} else {
printf("ERREUR: Nouvelle valeure refusée !\n");
fflush(stdout);
}
} else {
printf("Commande inaccessible dans cet état !\n");
fflush(stdout);
}
}
//---------------- Traitons en second les commandes courtes -------------
//------------------------------------------------
//-------------- COMMANDE NAVIG. -----------------
//------------------------------------------------
else if (dst[0] == 'a') {
ConsLRTA[0] += PAS_L;
} else if (dst[0] == 'e') {
ConsLRTA[0] -= PAS_L;
} else if (dst[0] == 'q') {
ConsLRTA[1] += PAS_RT;
} else if (dst[0] == 'd') {
ConsLRTA[1] -= PAS_RT;
} else if (dst[0] == 'z') {
ConsLRTA[2] -= PAS_RT;
} else if (dst[0] == 's') {
ConsLRTA[2] += PAS_RT;
} else if (dst[0] == 'r') {
ConsLRTA[3] += PAS_A;
} else if (dst[0] == 'f') {
ConsLRTA[3] -= PAS_A;
}
//------------------------------------------------
//------------- COMMANDES VEQ --------------------
//------------------------------------------------
else if (dst[0] == 'p') {
VEqui += 50.0F;
printf("Nouvelle vitesse d'équilibre: %f\n", VEqui);
fflush(stdout);
if (state == 4) {
Vactu = VEqui;
}
} else if (dst[0] == 'm') {
VEqui -= 50.0F;
printf("Nouvelle vitesse d'équilibre: %f\n", VEqui);
fflush(stdout);
if (state == 4) {
Vactu = VEqui;
}
}
//------------------------------------------------
//------------- COMMANDES A_VOL --------------------
//------------------------------------------------
else if (dst[0] == '$') {
ALTITUDE_FIN_DECOLLAGE += 0.05F;
if(ALTITUDE_FIN_DECOLLAGE > 0.8F) {
ALTITUDE_FIN_DECOLLAGE = 0.8F;
}
printf("Nouvelle altitude cible au décollage: %icm\n", (int)(100.0F * ALTITUDE_FIN_DECOLLAGE));
fflush(stdout);
} else if (dst[0] == '*') {
ALTITUDE_FIN_DECOLLAGE -= 0.05F;
if(ALTITUDE_FIN_DECOLLAGE < 0.3F) {
ALTITUDE_FIN_DECOLLAGE = 0.3F;
}
printf("Nouvelle altitude cible au décollage: %icm\n", (int)(100.0F * ALTITUDE_FIN_DECOLLAGE));
fflush(stdout);
}
//------------------------------------- MACHINE ETAT -------------------------------
//------------------------------------------------
//--------------- COMMANDE "w" -----------------
//------------------------------------------------
else if (dst[0] == 'w') {
if (state == -1) {
printf("Erreur ignoré !\n");
state = 0;
} else {
printf("ERREUR: La commande ne peut étre appliqué dans ce contexte.\n");
}
fflush(stdout);
}
//------------------------------------------------
//--------------- COMMANDE "x" -----------------
//------------------------------------------------
else if (dst[0] == 'x') {
if (state == -1 || state == 0 || state == 1) {
printf("Initialisation... OK\n");
//--RAZ attitude et position
Centrale_RAZAttitude();
//Centrale_RAZPosition();
videBufferSonar();
//--RAZ des consignes
ConsLRTA[0] = 0.0F; // 0 en lacet
ConsLRTA[3] = 0.0F; // 0 en altitude
if (ConsLRTA[1] != 0.0F || ConsLRTA[2] != 0) {
printf("\n\nWARNING: Initialisé avec consignes RT non nulles !!\n\n");
}
if (state == -1) {
printf("Il faut encore faire 'clr'\n");
} else {
printf("Pret pour mise en rotation\n");
state = 1;
}
fflush(stdout);
} else {
printf("ERREUR: La commande ne peut étre appliqué dans ce contexte.\n");
}
fflush(stdout);
}
//------------------------------------------------
//--------------- COMMANDE "c" -----------------
//------------------------------------------------
else if (dst[0] == 'c') {
if (state == 1) {
printf("Mise en rotation...\n");
state = 2;
} else {
printf("ERREUR: La commande ne peut étre appliqué dans ce contexte.\n");
}
fflush(stdout);
}
//------------------------------------------------
//--------------- COMMANDE "v" -----------------
//------------------------------------------------
else if (dst[0] == 'v') {
if (state == 2) {
printf("Décollage...\n");
Vactu = 0.0F;
state = 3;
} else {
printf("ERREUR: La commande ne peut étre appliqué dans ce contexte.\n");
}
fflush(stdout);
}
//------------------------------------------------
//--------------- COMMANDE "b" -----------------
//------------------------------------------------
else if (dst[0] == 'b') {
if (state == 4 || state == 3) {
printf("Atterissage...\n");
state = 5;
} else {
printf("ERREUR: La commande ne peut étre appliqué dans ce contexte.\n");
}
fflush(stdout);
}
//------------------------------------------------
//--------------- COMMANDE "STOP" ----------------
//------------------------------------------------
else if (dst[0] == ' ') {
state = -1;
printf("\n !! ARRET D'URGENCE !! \n");
fflush(stdout);
}
//------------------------------------- Divers ---------------
//------------------------------------------------
//--------------- COMMANDE "k" ----------------
//------------------------------------------------
else if (dst[0] == 'k') {
printf("Arret du programme...\n");
fflush(stdout);
break; // Termine la boucle et entraine la fermeture du programme
}
//------------------------------------------------
//--------------- COMMANDE "n" -----------------
//------------------------------------------------
else if (dst[0] == 'n') {
dispType++;
if (dispType == 3) {
dispType = 0;
}
printf("Mode d'affichage changé. (mode actuel: %i)\n",
dispType);
fflush(stdout);
}
//------------------------------------------------
//--------------- COMMANDE "j" -----------------
//------------------------------------------------
else if (dst[0] == 'j') {
modeI2C++;
if (modeI2C == 2) {
modeI2C = 0;
}
printf(
"Mode de controle I2C changé. (mode actuel: %i)\n",
modeI2C);
fflush(stdout);
}
//------------------------------------------------
//--------------- COMMANDE "u" -----------------
//------------------------------------------------
else if (dst[0] == 'u') {
BUF_CTR++;
if (BUF_CTR == 3) {
BUF_CTR = 1;
}
printf(
"Buffer centrale changé. (facteur actuel: %i)\n",
BUF_CTR);
fflush(stdout);
}
//------------------------------------------------
//--------------- COMMANDE "y" -----------------
//------------------------------------------------
else if (dst[0] == 'y') {
if (readSonar == 1) {
readSonar = 0;
printf("Lecture sonar désactivée !\n");
} else {
readSonar = 1;
printf("Lecture sonar activée !\n");
}
fflush(stdout);
}
//------------------------------------------------
//--------------- COMMANDE "i" ----------------
//------------------------------------------------
else if (dst[0] == 'i') {
dispFreqOnce = 1;
}
//------------------------------------------------
//-------------- COMMANDE "l" ---------------
//------------------------------------------------
else if (dst[0] == 'l') {
// Listons les réglages PID actuels
printf("\n Proportionnel Intégral Dérivé\n");
printf("Lacet : %15f %15f %15f\n", GAINS_PRO[0],
GAINS_INT[0], GAINS_DER[0]);
printf("Roulis : %15f %15f %15f\n", GAINS_PRO[1],
GAINS_INT[1], GAINS_DER[1]);
printf("Tangage : %15f %15f %15f\n", GAINS_PRO[2],
GAINS_INT[2], GAINS_DER[2]);
printf("Altitude : %15f %15f %15f\n", GAINS_PRO[3],
GAINS_INT[3], GAINS_DER[3]);
printf("Vequi : %15f", VEqui);
fflush(stdout);
}
//------------------------------------------------
//---------------- COMMANDE "HELP" ---------------
//------------------------------------------------
else if (dst[0] == 'h') {
Misc_PrintHelp();
fflush(stdout);
}
//------------------------------------------------
//------------- COMMANDE NON RECONNUE ------------
//------------------------------------------------
else {
printf("\nCommande non reconnue: %s", cptr); // Pas de \n car cptr en contient un !
printf("\nAppuyez sur 'h' pour obtenir de l'aide.\n");
fflush(stdout);
}
}
}
}
//////////////////////////////////////////////////////////////////
//
// Récupération de l'état du systéme
//
//////////////////////////////////////////////////////////////////
if (state != -1) {
//
// On attend une nouvelle trame centrale
//
attenteCentrale = 0;
for (;;) {
appelsCentrale++;
Centrale_CheckData(1);
if (NouvelleTrameCentrale) {
NouvelleTrameCentrale = 0;
break;
//--La trame a été récupéré et analysé, nous avons alors la nouvelle attitude du drone
// La variable trameRate a été mise a 1 si on a raté une trame entre temps !
} else {
attenteCentrale++;
if (attenteCentrale > 1000 * 100) { // La valeur de 100 convient parfaitement en lecture bloquante
printf("\nERREUR: Pas de données centrale !\n");
fflush(stdout);
state = -1;
break;
}
}
}
}
dernierTestSonar++;
if (readSonar && state != -1 && dernierTestSonar >= TEST_SONAR_TOUT_LES
&& !trameRate) {
dernierTestSonar = 0;
//
// On regarde si on a une trame sonar
//
appelsSonar++;
Sonar_CheckData(1);
if (NouvelleTrameSonar) {
NouvelleTrameSonar = 0;
attenteSonar = 0;
//--Tant mieux, l'altitude a été mis-é-jour
if (SonarTropBas) {
if (state == 4) {
printf("\nDANGER: Trop bas");
fflush(stdout);
}
} else if (SonarTropHaut) {
if (state == 4) {
printf("\nDANGER: Trop haut, baisse consigne");
fflush(stdout);
ConsLRTA[3] -= 0.005F; // 0.5cm 20 fois par seconde => 10cm / s
}
} else {
// On est dans la plage du sonar !
}
} else {
//--Tant pis, nous avons toujours l'ancienne valeur
attenteSonar++;
if (attenteSonar > ATTENTE_SONAR_MAXI) {
printf("\nERREUR: Pas de données sonar !\n");
fflush(stdout);
state = -1;
}
}
}
//////////////////////////////////////////////////////////////////
//
// Calcul de l'intervalle de temps
//
//////////////////////////////////////////////////////////////////
//
// Important:
//
// A ce stade de la boucle, nous avons récupéré une trame centrale, et en avons extrait l'information
// temporelle qui nous a permis de calculer l'intervalle de temps réel entre les deux trames.
// Cette intervalle est stocké dans "IntervalleTemps" et est utilisé par Asservissement_Controle()
// et puis par Pilote_CalculConsignes();
//
//
// Calcul de l'intervalle de temps avec les trames centrale
//
tempsSCum += IntervalleTemps;
float TOUT_LES = 0.25F;
if (tempsSCum >= TOUT_LES) {
tempsSCum -= TOUT_LES;
dispUneFois = 1;
}
/*if (0) { // Clignottement LED verte: TS7500
if (tempsSCum > 0.5F && !grnLedOn) {
grnLedOn = 1;
Misc_SetGrnLed(grnLedOn);
} else if (tempsSCum < 0.5F && grnLedOn) {
grnLedOn = 0;
Misc_SetGrnLed(grnLedOn);
}
}
*/
//////////////////////////////////////////////////////////////////
//
// Transitions d'état automatiques et sécurités
//
//////////////////////////////////////////////////////////////////
// 3 -> 4 (Fin du décollage)
if (state == 3) {
if (Vactu < VEqui) {
// Premiere phase du décollage: amener Vactu a VEqui
Vactu += RAMPE_ROTATION * IntervalleTemps;
if (Vactu >= VEqui) {
Vactu = VEqui;
printf("\nPuissance de vol atteinte ! Ascension verticale...\n");
fflush(stdout);
}
} else {
// Seconde phase: ascension verticale
ConsLRTA[3] += RAMPE_ASCENSION * IntervalleTemps;
if (ConsLRTA[3] >= ALTITUDE_FIN_DECOLLAGE) {
ConsLRTA[3] = ALTITUDE_FIN_DECOLLAGE;
state = 4;
printf("\nDécollage terminé ! Vol de croisiére.\n");
fflush(stdout);
}
}
}
// 5 -> 1 (Fin de l'atterissage)
if (state == 5) {
if (ConsLRTA[3] > 0.0F) {
// Premiere phase: Descente verticale
ConsLRTA[3] -= RAMPE_DESCENTE * IntervalleTemps;
if (ConsLRTA[3] <= 0.0F) {
ConsLRTA[3] = 0.0F;
printf("\nDescente terminée !\n");
fflush(stdout);
}
} else {
// Seconde phase: extinction des moteurs
Vactu -= RAMPE_ROTATION * IntervalleTemps;
if (Vactu <= 0.0F) { // Sera limité par la valeur minimale de toute faéon
Vactu = 0.0F;
state = 1;
printf("\nAtterissage terminé !\n");
fflush(stdout);
}
}
}
//
// Vérification des plages de fonctionnement
//
if (state >= 1) { // Initialisé ou en vol
//--Angle de lacet
if (Misc_Abs(ConsLRTA[0] - PosLRTA[0]) > ECART_MAXI_L) {
printf(
"\nERREUR: Angle de lacet hors de porté (Cons=%5fé, Pos=%5fé)!\n",
57 * ConsLRTA[0], 57 * PosLRTA[0]);
fflush(stdout);
state = -1;
}
//--Angle de roulis
if (Misc_Abs(ConsLRTA[1] - PosLRTA[1]) > ECART_MAXI_RT) {
printf(
"\nERREUR: Angle de roulis hors de porté (Cons=%5fé, Pos=%5fé)!\n",
57 * ConsLRTA[1], 57 * PosLRTA[1]);
fflush(stdout);
state = -1;
}
//--Angle de tangage
if (Misc_Abs(ConsLRTA[2] - PosLRTA[2]) > ECART_MAXI_RT) {
printf(
"\nERREUR: Angle de tangage hors de porté (Cons=%5fé, Pos=%5fé)!\n",
57 * ConsLRTA[2], 57 * PosLRTA[2]);
fflush(stdout);
state = -1;
}
//--Altitude
if (Misc_Abs(ConsLRTA[3] - PosLRTA[3]) > ECART_MAXI_A) {
printf(
"\nERREUR: Altitude hors de porté (Cons=%5fcm, Pos=%5fcm)!\n",
100 * ConsLRTA[3], 100 * PosLRTA[3]);
fflush(stdout);
state = -1;
}
}
//////////////////////////////////////////////////////////////////
//
// Calcul et envoi de la commande
//
//////////////////////////////////////////////////////////////////
if (state == -1 || state == 0 || state == 1) {
// Aucune rotation
Controlleur_Envoi(0);
} else if (state == 2) {
// Rotation lente
Controlleur_Envoi(1);
} else if (state == 3 || state == 4 || state == 5) {
//Pilote_CalculConsignes();
Asservissement_Controle();
Controlleur_Envoi(2);
}
//////////////////////////////////////////////////////////////////
//
// Affichage de quelques infos
//
//////////////////////////////////////////////////////////////////
if (dispUneFois) {
dispUneFois = 0;
//
// Mise a jour LED rouge TS7500
//
/*if (state == -1 && !redLedOn) {
redLedOn = 1;
Misc_SetRedLed(redLedOn);
} else if (state >= 0 && redLedOn) {
redLedOn = 0;
Misc_SetRedLed(redLedOn);
}
*/
//
// dispType :
// 0 : aucun affichage régulier
// 1 : affichage ligne d'état
// 2 : affichage ligne d'état + freqs
if (dispType > 0) {
//
// Mode I2C
//
printf("\n[M=%i]", modeI2C);
//
// Etat
//
if (state == -1) {
printf("[ERR]");
} else if (state == 0) {
printf("[N.I]");
} else if (state == 1) {
printf("[PRT]");
} else if (state == 2) {
printf("[ROT]");
} else if (state == 3) {
printf("[DEC]");
} else if (state == 4) {
printf("[VOL]");
} else if (state == 5) {
printf("[ATT]");
}
//
// Vmoyen et equi
//
printf("[%i/%i/%i]", Controlleur_VMoyen(), (int)(VEqui),
Ass_CorAlti());
//
// Sonar
//
printf("A: ");
if (SonarTropBas) {
printf("T.BAS");
} else if (SonarTropHaut) {
printf("T.HAUT");
} else {
printf("%3i", (int)(100 * PosLRTA[3]));
}
printf("/%3icm", (int)(100 * ConsLRTA[3]));
//
// Lacet, roulis, tangage
//
printf(" L: %3i/%3ié R: %2i/%2ié T: %2i/%2ié", (int)(57
* PosLRTA[0]), (int)(57 * ConsLRTA[0]), (int)(57
* PosLRTA[1]), (int)(57 * ConsLRTA[1]), (int)(57
* PosLRTA[2]), (int)(57 * ConsLRTA[2]));
//
// Commande
//
// if (dispType == 3) {
// Controlleur_PrintCmd();
// }
if (dispType == 2 || dispFreqOnce) {
printf(
"\nCENTRALE: Ap: %5i Tr: %4i Er: %4i Re: %4i Ef: %4i",
appelsCentrale, freqCentrale, erreursCentrale,
recherchesCentrale, tramesEffacees);
printf(
"\nSONAR : Ap: %5i Tr: %4i Er: %4i Re: %4i Ef: %4i",
appelsSonar, freqSonar, erreursSonar,
recherchesSonar, 0);
dispFreqOnce = 0;
}
fflush(stdout);
}
freqSonar = 0;
appelsSonar = 0;
erreursSonar = 0;
recherchesSonar = 0;
freqCentrale = 0;
appelsCentrale = 0;
erreursCentrale = 0;
recherchesCentrale = 0;
tramesEffacees = 0;
}
//////////////////////////////////////////////////////////////////
//
// Fin de la boucle, temporisation si nécessaire
//
//////////////////////////////////////////////////////////////////
if (state == -1) {
usleep(10*1000);
}
} // Fin de la boucle infinie
//---------------------------------------------------------------------------
//
//
// FERMETURE DU PROGRAMME: LIBERATION MEMOIRE
//
//
//---------------------------------------------------------------------------
//--Envoi d'une consigne zero
Controlleur_Envoi(0);
printf("Fermeture du programme...\n");
fflush(stdout);
Sonar_Termine();
Centrale_Termine();
Controlleur_Termine();
Pilote_Termine();
CommWiFi_Termine();
//Misc_SetGrnLed(0); TS7500
//Misc_SetRedLed(0);
printf("Programme terminé !\n");
fflush(stdout);
return 1;
}
