/*!
* \brief VideoService::reconnect stops the timer and flushes the socket.
* The connectToDrone() method will be called after 500ms.
*/
void VideoService::reconnect()
{
if(timer->isActive())
timer->stop();
capture->release();
delete capture;
QThread::msleep(500);
capture = new cv::VideoCapture();
connectToDrone();
}
/*!
* \brief VideoService::provision is called when the object thread is started.
* The service will call connectToDrone() and starts the lostTimer.
*/
void VideoService::provision()
{
connectToDrone();
lostTimer->start();
}
/** \fn void main(void)
\ingroup Rabbit
\brief Fonction principal : gestion du pilotage du drone en fonctions des entrées/sorties
\author Baudouin Feildel
\author Thibaut Marty
**/
void main(void)
{
etat_commandes ec;
ardrone droneV;
ardrone* drone = &droneV;
char cptPassErr = 0;
wifi_status status;
int initOk;
/// Initialise les ports de la carte et les entrées/sorties
brdInit(); // fonction de base de Rabbit
BRDInit(); // fonction pour les bits utilisés
init_etat_commandes(&ec);
lireCommandes(&ec);
/// Crée la structure du drone
newARDrone(drone);
/// Initialise la connexion (tant que toutes les étapes ne sont pas réussies) :
do
{
printf("tentative d'initialisation\n");
initOk = 0;
/// .... Initialise le driver TCP
if(sock_init())
printf("erreur de sock_init()\n");
else
initOk++;
/// .... Initialise la socket de connexion avec le drone
if(!connectToDrone(drone))
printf("erreur connectToDrone()\n");
else
initOk++;
/// .... Se connecte au réseau WiFi
connexion(&ec);
/// .... Initialise le drone
if(!initDrone(drone, &ec))
printf("erreur initDrone()\n");
else
initOk++;
} while(initOk < 3);
printf("tentative d'initialisation reussie\n");
/// Vérifie que l'on n'est pas déjà en position 'vol', sinon indique à l'utilisateur de changer le switch en faisant clignoter la LED erreur
do
{
costate
{
lireCommandes(&ec);
yield;
}
costate // Fait clignoter la LED d'erreur
{
ec.led_erreur = ec.led_erreur ? 0 : 1;
ecrireCommandes(&ec);
waitfor(DelayMs(300));
}
} while(ec.switch_land == 0);
ec.led_erreur = 0;
ecrireCommandes(&ec);
/// Boucle principale (déroulement) :
for(;;)
{
/// .... Tâche de gestion des entrées/sorties et de l'envoi de commandes au drone
costate
{
tcp_tick(NULL);
// Lit les entrées
lireCommandes(&ec);
/// ........ Si le bouton d'arrêt d'urgence est actif :
if(!ec.bp_arret_urgence)
{
/// ............ Envoie la commande d'arrêt d'urgence
aru(drone);
drone->fly = false;
/// ............ Attend le relâchement du bouton en faisant clignoter la LED debug
do
{
costate
{
lireCommandes(&ec);
yield;
}
costate // Fait clignoter la LED de debug
{
ec.led_debug = ec.led_debug ? 0 : 1;
ecrireCommandes(&ec);
waitfor(DelayMs(100));
}
yield; // Pour la tâche de vérification de la connexion WiFi
} while(!ec.bp_arret_urgence);
ec.led_debug = 0;
ecrireCommandes(&ec);
/// ............ Renvoie la commande (pour sortir du mode d'arrêt d'urgence)
aru(drone);
}
/// ........ Sinon (traitement de tous les cas position du switch / vol en cours) :
else
{
// pour plus tard :
// if(ec.bp_video) { }
/// ............ Si le bouton trim est actif et que l'on n'est pas en cours de vol, réinitialise le drone
if(!ec.bp_trim && !drone->fly)
initDrone(drone, &ec);
/// ............ Si le switch est en position haute et que l'on ne vole pas
if(ec.switch_land == 0 && !drone->fly)
{
/// ................ Fait décoler le drone. S'il y a une erreur : attend que l'utilisateur repasse le switch en position basse en faisant clignoter la LED erreur
if(!(initDrone(drone, &ec) && takeoff(drone)))
{
do
{
costate
{
lireCommandes(&ec);
yield;
}
costate // Fait clignoter la LED d'erreur
{
ec.led_erreur = ec.led_erreur ? 0 : 1;
ecrireCommandes(&ec);
waitfor(DelayMs(100));
}
} while(ec.switch_land == 0);
}
}
/// ............ Si le switch est en position basse et que l'on vole
else if(ec.switch_land == 1 && drone->fly)
{
/// ................ Fait atterrir le drone. S'il y a une erreur : attend que l'utilisateur passe le switch en position haute
if(!land(drone))
{
do
{
costate
{
lireCommandes(&ec);
yield;
}
costate // Fait clignoter la LED d'erreur
{
ec.led_erreur = ec.led_erreur ? 0 : 1;
ecrireCommandes(&ec);
waitfor(DelayMs(300));
}
} while(ec.switch_land == 1);
}
}
/// ............ Les autres cas sont normaux et ne nécessitent pas de traitement particulier
/// ............ Si on est en vol :
if(drone->fly)
{
/// ................ Traite la valeur des joysticks et la stock dans la structure ardrone
setGoUpDown(drone, (float)(ec.joystick_2x - 2048) / 2048);
setTurnLeftRight(drone, (float)(ec.joystick_2y - 2048) / 2048);
setTiltFrontBack(drone, (float)(ec.joystick_1x - 2048) / 2048);
setTiltLeftRight(drone, (float)(ec.joystick_1y - 2048) / 2048);
/// ................ Envoie la commande avec les valeurs : s'il y a une erreur on incrémente un compteur d'erreurs. Au delà de dix erreurs de suite on tente de faire atterir le drone. S'il n'y a pas d'erreurs, on attend 30ms avant le prochain envoi
if(!(volCommand(drone, drone->tiltLeftRight, drone->tiltFrontBack, drone->goUpDown, drone->turnLeftRight)))
{
if(cptPassErr > 10)
land(drone);
else
cptPassErr++;
}
else
{
cptPassErr = 0; // Remise à zéro du compteur d'erreur en cas de réussite
waitfor(DelayMs(30)); // prochain envoi de commande dans 30 ms
}
}
else
yield;
}