void CRoblind::boucleMouvement() { switch (m_config.mode) { case manuel: modeManuel(); break; case semiAuto: modeSemiAuto(); break; case automatique: // TODO break; } }
void boucleGestionVol() { char retournement = 0; char anglesTimeout, capteursTimeout, telecommandeTimeout, batteryTimeout = 0; long lastCapteursDump = 0; char cnt_INU = 0, i = 0, cnt_Capteurs = 0, affCapteursEtat = FALSE; //Note : on démarre en mode Manuel. Les transistions entre modes sont gérées dans les fonctions de mode modeVol = MANUEL; //Chargement des coefficients PID et des offsets stockés en EEPROM readEEPROM(&coeffsPID, EEPROM_BASE, MAX_PID * sizeof(COEFFS_PID)); readEEPROM(&telecommandeOffsets, EEPROM_BASE + EEPROM_OFFSET_TRIMS, sizeof(TELECOMMANDE_DATA)); readEEPROM(&anglesOffsets, EEPROM_BASE + EEPROM_OFFSET_ANGLES, sizeof(IMU_ANGLES_VITESSES)); //Teste si les données stockées en EEPROM sont valides : on vérifie que le premier coefficient PID est différent de 0xFFFFFFFF (NaN) if (*((long*)(&coeffsPID[0].kp)) == 0xFFFFFFFF) { initEEPROM(); } //Appel de la fonction d'initialisation du vol : réglage des consignes en mode failsafe, initialisation des coefficients des PID, ... //defaultPID(); //defaultTrim(); /*** Affichage des PIDs ***/ printf("> PID_roulis : p%.5f i%.5f d%.5f\r\n", (double)coeffsPID[ROULIS].kp, (double)coeffsPID[ROULIS].ki, (double)coeffsPID[ROULIS].kd ); printf("> PID_tangage : p%.5f i%.5f d%.5f\r\n", (double)coeffsPID[TANGAGE].kp, (double)coeffsPID[TANGAGE].ki, (double)coeffsPID[TANGAGE].kd ); printf("> PID_lacet : p%.5f i%.5f d%.5f\r\n", (double)coeffsPID[LACET].kp, (double)coeffsPID[LACET].ki, (double)coeffsPID[LACET].kd ); printf("> PID_vitesse : p%.5f i%.5f d%.5f\r\n", (double)coeffsPID[VITESSE].kp, (double)coeffsPID[VITESSE].ki, (double)coeffsPID[VITESSE].kd ); printf("> PID_altitude : p%.5f i%.5f d%.5f\r\n", (double)coeffsPID[ALTITUDE].kp, (double)coeffsPID[ALTITUDE].ki, (double)coeffsPID[ALTITUDE].kd ); //Affichage du niveau de batterie restant printf("Battery level : %.1f %%\n", (double)read_Battery_Level() ); while (1) { //Test des timeouts de communication anglesTimeout = (globalTime - lastAnglesMAJ) > ANGLES_TIMEOUT; capteursTimeout = (globalTime - lastCapteursMAJ) > CAPTEURS_TIMEOUT; telecommandeTimeout = (globalTime - lastTelecommandeMAJ) > TELECOMMANDE_TIMEOUT; batteryTimeout = (globalTime - lastBatteryMAJ) > BATTERY_TIMEOUT; //Traite les commandes reçues par le XBeeeeeeeeeeeeeeee traiterCommandes(); /*** Affichage de l'état de fonctionnement des capteurs ***/ if( affCapteursEtat == FALSE && capteursMAJ ){ if( CAPTEUR_BAROMETRE_OK ){ printf("> Barometre \t-> ON\n"); } else{ printf("> Barometre \t-> OFF\n"); } if( CAPTEUR_PITOT_OK ){ printf("> Pitot \t-> ON\n"); } else{ printf("> Pitot \t-> OFF\n"); } if( CAPTEUR_ULTRASON_OK ){ printf("> Ultrason \t-> ON\n"); } else{ printf("> Ultrason \t-> OFF\n"); } if( CAPTEUR_GPS_OK ){ printf("> GPS \t\t-> ON\n"); } else{ printf("> GPS \t\t-> OFF\n"); } //On ne fait l'affichage qu'une fois au démarrage affCapteursEtat = TRUE; } //Transitions de tous les modes vers le failsafe /* if (anglesMAJ) { retournement = ((angles.roulis>1.3) || (angles.roulis<-1.3) || (angles.tangage>1.3) || (angles.tangage<-1.3)); if (retournement) { modeVol = FAILSAFE; printf("-> FAILSAFE (retournement)\r\n"); } }*/ /*if (globalTime - lastCapteursDump > 500) { lastCapteursDump = globalTime; printf ("Ab(m) : %d Au(cm) : %d P(cm/s) : %u GPSx : %lu GPSy : %lu J : %u\r\n", capteurs.altitude_baro, capteurs.altitude_us, capteurs.vitesse, capteurs.gps_x, capteurs.gps_y, capteurs.jauge); }*/ //Test de timeout sur la réception des trames de l'IMU if ((modeVol == SEMI_AUTO || modeVol == AUTO) && anglesTimeout) { modeVol = FAILSAFE; if( cnt_INU == 0 ) { // affichage uniquement une fois par RESET de la communication IMU printf("-> FAILSAFE (timeout angles)\r\n"); } cnt_INU++; } //Test de timeout sur la réception des trames télécommande //! TODO : à changer pour les essais hors portée de télécommande if ((modeVol == MANUEL || modeVol == SEMI_AUTO) && telecommandeTimeout) { modeVol = FAILSAFE; printf("-> FAILSAFE (timeout telecommande)\r\n"); LEDV1 = 0; } #if TYPE_DRONE == MULET_POLY //Test de timeout sur la réception des trames capteurs if((modeVol == SEMI_AUTO || modeVol == AUTO) && capteursTimeout) { modeVol = FAILSAFE; printf("-> FAILSAFE (timeout capteurs)\r\n"); cnt_Capteurs++; } //Reset du SPI si on a pas de MAJ capteurs if( cnt_Capteurs == 10 ) { SPI1STATbits.SPIEN = 0; // SPI désactivé printf("-> RESET SPI\r\n"); SPI1STATbits.SPIEN = 1; // SPI activé cnt_Capteurs = 0; } #else /*Le quadri et le drone orange n'ont pas besoins d'avoir les données / des capteurs pour les phases de test. */ #endif //Reset de l'UART si on n'a pas de MAJ des angles if( cnt_INU == 10 ) { U2MODEbits.UARTEN = 0; // Reset UART2 printf("-> RESET UART2\r\n"); U2MODEbits.UARTEN = 1; // Activer UART2 cnt_INU = 0; } //Infos sur le niveau de batterie //!\ Uniquement pour la 2 éléments alimentant la carte mère if( batteryTimeout ) { printf("Battery level : %.1f %%\n", (double)read_Battery_Level() ); } switch (modeVol) { case FAILSAFE: modeFailSafe(); prevModeVol = FAILSAFE; //On sort du mode failsafe si on reçoit une trame télécommande valide if (telecommandeMAJ && !retournement) { telecommandeMAJ = 0; printf("-> MANUEL\r\n"); modeVol = MANUEL; LEDV1 = 1; } break; case MANUEL: if (telecommandeMAJ) { modeManuel(); prevModeVol = MANUEL; telecommandeMAJ = 0; if (telecommande.voie[V_MODESEMIAUTO] <50 && anglesMAJ) { modeVol = SEMI_AUTO; printf("-> SEMIAUTO\r\n"); } /*** Pour le RESET de la com IMU ***/ if( telecommande.voie[V_MODESEMIAUTO] <50 && !anglesMAJ ) { if( cnt_INU == 0 ) { printf("/!\\ Pas de donnees centrale \r\n"); } cnt_INU++; } #if TYPE_DRONE == MULET_POLY /*** Pour le RESET de la com carte capteur ***/ if( telecommande.voie[V_MODESEMIAUTO] <50 && !capteursMAJ ) { if( cnt_Capteurs == 0 ) { printf("/!\\ Pas de donnees capteurs \r\n"); } cnt_Capteurs++; } #else /*Le quadri et le drone orange n'ont pas besoins d'avoir les données / des capteurs pour les phases de test. */ #endif } break; case SEMI_AUTO: //Le calcul des intégrateurs de l'asserv se fait avec le temps réel écoulé, qui est donné par la centrale inertielle if (anglesMAJ) { modeSemiAuto(); prevModeVol = SEMI_AUTO; cnt_INU = 0; anglesMAJ = 0; } if (telecommande.voie[V_MODESEMIAUTO] >=50) { printf("-> MANUEL\r\n"); modeVol = MANUEL; } if (telecommande.voie[V_MODEAUTO] >= 50 && PITOT_MAJ && BAROMETRE_MAJ ) { //Pour le test semi auto altitude printf("-> AUTO\r\n"); modeVol = AUTO; } break; case AUTO: if (anglesMAJ) { modeAuto(); prevModeVol = AUTO; anglesMAJ = 0; } if (telecommande.voie[V_MODEAUTO] < 50) { printf("-> SEMIAUTO\r\n"); modeVol = SEMI_AUTO; } if (telecommande.voie[V_MODESEMIAUTO] >= 50) { printf("-> MANUEL\r\n"); modeVol = MANUEL; } break; } } }