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);
}
}
}
}
int find_hamiltonian(int argc, char* argv[]) {
int i, p;
int id1, id2, ret;
MesgOrder message;
MesgInfo reply;
if (argc < 1) print_usage_info(argv[0]);
message.type = getpid();
message.command = FIND;
message.len = argc - 2;
for (i = 0; i < message.len; i++) {
if ((p = myatoi(argv[i + 2])) < 0)
print_usage_info(argv[0]);
message.data[i] = p;
}
id1 = init_queue(MKEY1, 0);
send_order(id1, &message, argc * sizeof(int));
id2 = init_queue(MKEY2, 0);
receive_info(id2, &reply, getpid());
printf("H %d\n", reply.command);
if (reply.command == -1) ret = -1;
if (reply.command == 0) ret = 1;
if (reply.command > 0) ret = 0;
return ret;
}
int add_edge(int argc, char* argv[]) {
int V, W, G;
int id1, id2;
MesgOrder message;
MesgInfo reply;
if (argc != 5)
print_usage_info(argv[0]);
if ((V = myatoi(argv[2])) < 0)
print_usage_info(argv[0]);
if ((W = myatoi(argv[3])) < 0)
print_usage_info(argv[0]);
if ((G = myatoi(argv[4])) <= 0)
print_usage_info(argv[0]);
message.type = getpid();
message.command = ADD;
message.len = 3;
message.data[0] = V;
message.data[1] = W;
message.data[2] = G;
id1 = init_queue(MKEY1, 0);
send_order(id1, &message, 5 * sizeof(int));
id2 = init_queue(MKEY2, 0);
receive_info(id2, &reply, getpid());
return reply.command;
}
/**
* @brief This function contains the implementation of the strategy
* @param[in] pw unique instance of the structure PW
*/
void do_turn(struct PW *pw)
{
struct Planet *src, *dst;
int ships;
if (pw->turnCount % 10 == 0) {
src = random_planet_owned_by(pw, ME);
dst = random_foreign_planet(pw);
if (src && dst) {
ships = src->ships / 2;
send_order(src->id, dst->id, ships);
}
}
}
/*
INPUT: angle (float)
tolerance (float)
This function will make uav turn to the direction of target with an error of +- tolerance
*/
void turn_angle2(float target_angle, float tol) {
fdata sauv_ndata = get_ndata();
float angle_360 = sauv_ndata.psi_current;
float angle_inf, angle_sup;
angle_inf = target_angle - tol;
angle_sup = target_angle + tol;
printf("inf [%f], sup[%f]\n",angle_inf,angle_sup);
if (sauv_ndata.psi_current < 0) angle_360 = 360 + sauv_ndata.psi_current;
if (target_angle < 0) target_angle += 360;
if (target_angle > angle_360) {
(target_angle - angle_360) < 180 ? send_order(turn_left,NULL) : send_order(turn_right,NULL);
} else {
(angle_360 - target_angle) < 180 ? send_order(turn_right,NULL) : send_order(turn_left,NULL);
}
while (!(sauv_ndata.psi_current > angle_inf && sauv_ndata.psi_current < angle_sup)) {
sauv_ndata = get_ndata();
printf("tourne [%f] hasta [%f]!\n\n",sauv_ndata.psi_current,angle_sup);
usleep(100);
if (target_angle > angle_360) {
(target_angle - angle_360) < 180 ? send_order(turn_left,NULL) : send_order(turn_right,NULL);
} else {
(angle_360 - target_angle) < 180 ? send_order(turn_right,NULL) : send_order(turn_left,NULL);
}
}
send_order(stop,NULL);
sleep(1);
printf("finish turning\n");
}
