/*
All steps for reaching target
1 Take off and Calibration
2 Receive coordinates gps and calculate direction
3 Turn to direction, then move forward in 3s
4 Return to step 2, if distance < 4 m , Landing ...
*/
void go_target() {
//1 DONE, 0 not yet
static int landed = 1;
static int calibration = 0;
static int mission = 0;
comm_datas datas;
comm_datas_target datas_target;
struct gps_coordinate depart;
struct gps_coordinate relatif_error;
//gps_error error_dest, error_depart;
mov speed;
static double distance,angle;
if ( (landed == 1) && (calibration == 0) ){
printf("start calibration\n");
calibrate_magneto(NULL);
sleep(4);
calibration = 1;
printf("Calibration done\n");
}
if ( (landed == 1) && (calibration == 1) && (mission == 0) ) {
datas = get_comm_datas();
extract_coord(datas.gprmc_string,&depart);
//printf("COORD UAV %f %f\n",depart.longitude,depart.latitude);
//extract_error(datas.gpgga_string,&error_depart);
datas_target = get_comm_datas_target();
//printf("COORD TG %f %f\n",datas_target.dest.longitude,datas_target.dest.latitude);
//extract_coord(datas_target.gprmc_string,&dest);
//extract_error(datas_target.gpgga_string,&error_dest);
if ((check_gps_coord_struc(&depart) > 0) && (check_gps_coord_struc(&datas_target.dest) > 0)) {
navigation(&depart, &datas_target.dest, &distance, &angle, NULL); //&relatif_error
printf("DISTANCE %f\n\n\n\n",distance);
printf("turn angle %f, &angle",angle);
turn_angle2(angle ,5.0);
if (distance > 5.0){
printf("La distance restante est %f\n",distance);
fprintf(redir_sortie,"La distance restante est %f\n",distance);
fflush(redir_sortie);
//send_order(forward,NULL);
speed.power = 3;
//speed.distance = distance/2;
//speed.time = d2time(speed.power,speed.distance);
send_order(forward,(void *)&speed);
sleep(3);
//sleep(4);
} else {
send_order(land,NULL);
printf("LANDING \n");
mission = 1;
//exit(0);
}
}
}
}
void go_target2() {
//1 DONE, 0 not yet
static int landed = 1;
static int calibration = 0;
static int mission = 0;
static float last_distance = 1000.0;
comm_datas datas;
comm_datas_target datas_target;
struct gps_coordinate depart;
struct gps_coordinate relatif_error;
mov speed;
static double distance,angle;
if ( (landed == 1) && (calibration == 0) ){
printf("start calibration\n");
calibrate_magneto(NULL);
sleep(4);
calibration = 1;
printf("Calibration done\n");
}
if ( (landed == 1) && (calibration == 1) && (mission == 0) ) {
datas = get_comm_datas();
extract_coord(datas.gprmc_string,&depart);
datas_target = get_comm_datas_target();
if ((check_gps_coord_struc(&depart) > 0) && (check_gps_coord_struc(&datas_target.dest) > 0)) {
navigation(&depart, &datas_target.dest, &distance, &angle, NULL); //&relatif_error
printf("DISTANCE %f\n\n\n\n",distance);
fprintf(redir_sortie,"DISTANCE %f\n\n\n\n",distance);
printf("turn angle %f, &angle",angle);
fprintf(redir_sortie,"turn angle %f, &angle",angle);
turn_angle2(angle ,5.0);
while (last_distance > distance) {
printf("La distance restante est %f\n",distance);
fprintf(redir_sortie,"La distance restante est %f\n",distance);
fflush(redir_sortie);
speed.power = 3;
send_order(forward,(void *)&speed);
last_distance = distance;
sleep(1);
}
if (distance < 10.0){
send_order(land,NULL);
printf("LANDING \n");
mission = 1;
fflush(redir_sortie);
exit(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;
}
}
