bool CRoblind::modeSemiAuto()
{
if (modeManuel() && !enEvitement) {
// Manoeuvre d'evitement
enEvitement = true;
// TODO
}
return true;
}
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;
}
}
}
