// --------------------------------------------------------------------------
// ARDrone::loopNavdata()
// Description : Thread function for Navdata.
// Return value : SUCCESS:0
// --------------------------------------------------------------------------
void ARDrone::loopNavdata(void)
{
while (1) {
// Get Navdata
if (!getNavdata()) break;
pthread_testcancel();
msleep(10);
}
}
// --------------------------------------------------------------------------
// ARDrone::loopNavdata()
// Thread function.
// Return value 0
// --------------------------------------------------------------------------
UINT ARDrone::loopNavdata(void)
{
while (flagNavdata) {
// Get Navdata
if (!getNavdata()) break;
Sleep(30);
}
// Disable thread loop
flagNavdata = 0;
return 0;
}
void SWITCH_DRONE_COMMANDE(int _order)
{
char message [64];
switch(_order){
case 0 :
reset_com(message);
break;
case 1 :
landing(message);
inC.flag_land = 0;
break;
case 2 :
//printf("SWITCH COMMANDE take off\n");
take_off(message);
inC.flag_takeoff = 0;
break;
case 3 :
//printf("case 3 : set_roll, roll_power=%f\n", roll_power);
set_roll(message, roll_move, roll_power);
inC.flag_rollcalled = 0;
break;
case 4 :
//printf("case 4 : set_pitch, pitch_power=%f, yaw_power=%f\n", pitch_power, yaw_power);
set_pitch(message, pitch_move, pitch_power);
inC.flag_pitchcalled = 0;
break;
case 5 :
set_yaw(message, yaw_move, yaw_power);
inC.flag_yawcalled = 0;
break;
case 6 :
set_roll(message, roll_move, roll_power);
set_pitch(message, pitch_move, pitch_power);
inC.flag_rollpitchcalled = 0;
break;
case 7 :
set_trim(message);
inC.flag_calibH = 0;
break;
case 8 :
calibrate_magneto(message);
sleep(3);
while(1)
{
set_yaw(message, RIGHT, 0.1);
while(isControllerReady()==0);
Main_Nav = getNavdata();
if (Main_Nav.magneto.heading_fusion_unwrapped > 0.0)
nav_suiv = Main_Nav.magneto.heading_fusion_unwrapped - 360.0;
else
nav_suiv = Main_Nav.magneto.heading_fusion_unwrapped + 360.0;
printf("angle_trouve et angle +/- 360 = %.2f, %.2f \r", Main_Nav.magneto.heading_fusion_unwrapped, nav_suiv);
if(nav_suiv < 0.0 && nav_prec > 0.0 || nav_suiv > 0.0 && nav_prec < 0.0)
{
inC.flag_calibM = 0;
printf("\nnav_suiv = %.2f | nav_prec = %.2f\n", nav_suiv, nav_prec);
break;
}
nav_prec = nav_suiv;
}
set_yaw(message, LEFT, 0.0);
inC.flag_calibM = 0;
break;
case 9 :
//set_emergency(message);
inC.flag_emergency = 0;
set_gaz(message, UP, 0.2);// this is an ugly hack (don't commit this)
break;
case 10 :
//anti_emergency(message);
inC.falg_antiemergency = 0;
set_gaz(message, UP, 0.0);// this is an ugly hack (don't commit this)
break;
case 20 :
calcul_mission();
break;
default :
printf("Scade ne marche pas si bien que ça finalement\n");
break;
}
}
Navdata return_navdata()
{
while(isControllerReady()==0); //attente données navdata actualisées
return getNavdata();
}
void* controlTask(void* arg)
{
pthread_mutex_t verrou_control;
pthread_cond_t cond;
pthread_cond_init(&cond, NULL);
pthread_mutex_init(&verrou_control, NULL);
struct timeval tp;
struct timespec ts;
float pitch_cmd = 0.0;
float roll_cmd = 0.0;
float angular_speed_cmd = 0.0;
float vertical_speed_cmd = 0.0;
initialize_at_com(); //initialisation du soket pour les commandes AT.
initNavdata(); //initialisation du soket pour les navdata.
Main_Nav = getNavdata();
//initialisation_uart(); //initialisation de la commuication Uart.
//creation of graph
FILE* file = fopen(NAME_MAP_DEMO, "r");
graph = createGraph(file);
fclose(file);
pthread_mutex_lock(&mutex_control);
inC.flag_control_e = CONTROL_ENABLED_SCADE;
inC.flag_control_s = STATE_MANUAL;
inC.flag_takeoff = 0;
inC.flag_land = 0;
inC.flag_emergency = 0;
inC.flag_calibH = 0;
inC.flag_calibM = 0;
inC.flag_pitchcalled = 0;
inC.flag_rollcalled = 0;
inC.flag_rollpitchcalled = 0;
inC.flag_yawcalled = 0;
system_state_machine_reset(&outC);
pthread_mutex_unlock(&mutex_control);
while(1)
{
//printf("Debut de la boucle while(1) du thread controlTask\n");
//récupération de la clock courante
gettimeofday(&tp, NULL);
ts.tv_sec = tp.tv_sec;
ts.tv_nsec = tp.tv_usec * 1000;
//application de la nouvelle période d'éxécution du thread_envoi_ordre
ts.tv_nsec += CONTROLTASK_PERIOD_CONTROLE_MS * 1000000;
ts.tv_sec += ts.tv_nsec / 1000000000L;
ts.tv_nsec = ts.tv_nsec % 1000000000L;
pthread_mutex_lock(&verrou_control);
//printf("Passage du pthread_mutex_lock(&verrou_control) dans controlTask\n");
pthread_mutex_lock(&mutex_control);
//printf("Passage du pthread_mutex_lock(&mutex_control) dans controlTask\n");
system_state_machine_reset(&outC);
system_state_machine(&inC,&outC); //CODE SCADE
//printf("ordre genere par la machine scade = %d\r", outC.order_called);
SWITCH_DRONE_COMMANDE(outC.order_called);//SWITCH pour l'envoi des ordres
pthread_mutex_unlock(&mutex_control);
pthread_cond_timedwait(&cond, &verrou_control, &ts);
pthread_mutex_unlock(&verrou_control);
}
}
