From 5126894634b9d285793f53607738fb453e400eb8 Mon Sep 17 00:00:00 2001 From: Florian Taillard Date: Tue, 14 Jun 2011 14:04:50 +0200 Subject: [PATCH] [ARDrone] Add support for multi-point of consigne --- owlps-ardrone/owlps-drone.c | 66 ++++++++++++++++++++++--------------- owlps-ardrone/owlps-drone.h | 6 ++++ 2 files changed, 45 insertions(+), 27 deletions(-) diff --git a/owlps-ardrone/owlps-drone.c b/owlps-ardrone/owlps-drone.c index 27ec9ed..949ddca 100644 --- a/owlps-ardrone/owlps-drone.c +++ b/owlps-ardrone/owlps-drone.c @@ -31,6 +31,10 @@ /* Options */ gps datagps; +point_consigne array_cons[30]; + +int i_consigne=0; +int num_point=0; struct timespec timestamp ; struct { @@ -99,7 +103,6 @@ int main(int argc, char *argv[]) FILE* log = NULL; float angle, distance, speed_pitch, distance_total; struct sigaction action; - int boot=0; program_name = argv[0] ; parse_command_line(argc, argv) ; @@ -158,6 +161,7 @@ int main(int argc, char *argv[]) fprintf(log, "Date;Heure;Latitude;Longitude;Latitude_cons;Longitude_cons;DistanceTotal;Distance;SpeedPitch;angle;x;y;x_retro;y_retro;x_cons;y_cons\n"); while(run) { + i_consigne=0; while(start==0) usleep(200000); //Wait GPS Signal @@ -176,32 +180,35 @@ int main(int argc, char *argv[]) drone_err(2); while(share_gps_cons.lat==-999.00||share_gps_cons.lon==-999.00); sleep(8); - distance_total=fabs(oc_distance_between(share_gps, share_gps_cons)); - - if(cmd_drone_init_direction()<0) - drone_err(3); - boot=0; - while(start==1&&!check_destination(share_gps, share_gps_cons)) + while(i_consigne<=num_point) { - if(read_position(TYPE)<0) - drone_err(1); + share_gps_cons.lat = array_cons[i_consigne].lat; + share_gps_cons.lon = array_cons[i_consigne].lon; + share_gps_cons.alt = array_cons[i_consigne].alt; - //calcul_trajectory(&angle, &distance_old); - angle = calcul_angle(); - distance = fabs(oc_distance_between(share_gps, share_gps_cons)); - if(speed_pitch<-(distance/distance_total)) - speed_pitch=speed_pitch; - else - speed_pitch=-(distance/distance_total); - if(speed_pitch < -1) speed_pitch = -1; - else if(speed_pitch > 1) speed_pitch = 1; + distance_total=fabs(oc_distance_between(share_gps, share_gps_cons)); + + while(start==1&&!check_destination(share_gps, share_gps_cons)) + { + if(read_position(TYPE)<0) + drone_err(1); + + //calcul_trajectory(&angle, &distance_old); + angle = calcul_angle(); + distance = fabs(oc_distance_between(share_gps, share_gps_cons)); + if(speed_pitch<-(distance/distance_total)) + speed_pitch=-(distance/distance_total)-0.2; - cmd_drone_move((speed_pitch/5), (angle/2)); - usleep(10000); - //boot++; - printf("Vitesse : %f Angle : %f\n", (speed_pitch/5), angle/2); - fprintf(log,"%d;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f\n",share_gps.date, share_gps.time, share_gps.lat, share_gps.lon, share_gps_cons.lat, share_gps_cons.lon, distance_total, distance, -(speed_pitch/2), angle/2, share_relatif.x, share_relatif.y, share_relatif_retro.x, share_relatif_retro.y, share_relatif_cons.x, share_relatif_cons.y); - //printf("[DEBUG] Vitesse %f Angle %f state %d\n",speed_pitch, angle,state_flying); + if(speed_pitch < -1) speed_pitch = -1; + else if(speed_pitch > 1) speed_pitch = 1; + + cmd_drone_move((speed_pitch), (angle)); + usleep(10000); + printf("Vitesse : %f Angle : %f\n", speed_pitch, angle); + fprintf(log,"%d;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f;%f\n",share_gps.date, share_gps.time, share_gps.lat, share_gps.lon, share_gps_cons.lat, share_gps_cons.lon, distance_total, distance, speed_pitch, angle, share_relatif.x, share_relatif.y, share_relatif_retro.x, share_relatif_retro.y, share_relatif_cons.x, share_relatif_cons.y); + //printf("[DEBUG] Vitesse %f Angle %f state %d\n",speed_pitch, angle,state_flying); + } + i_consigne++; } fclose(log); cmd_drone_landing(); @@ -209,7 +216,7 @@ int main(int argc, char *argv[]) sem_destroy(&lock); (void) close(socksendfd) ; (void) close(sockreceivefd) ; - + return 0 ; } @@ -417,6 +424,11 @@ void* thread_control(void* NULL_value) share_t_cons=atof(st); sprintf(response,"%sGeoSetpoint;OK;%.15f;%.15f;%f;%f\n",response,share_gps_cons.lat,share_gps_cons.lon,share_gps_cons.alt,share_t_cons); printf("[Consigne] Latitude %f Lontitude %f défini!\n",share_gps_cons.lat, share_gps_cons.lon); + array_cons[num_point].lat = share_gps_cons.lat; + array_cons[num_point].lon = share_gps_cons.lon; + array_cons[num_point].alt = share_gps_cons.alt; + num_point++; + printf("[Consigne %d point défini]\n", num_point); } else if (strcmp(cmd, "SendSetpoint")==0) { share_relatif_cons.x=atof(sx); @@ -436,10 +448,10 @@ void* thread_control(void* NULL_value) cmd_drone_landing(); sprintf(response,"%sLanding;OK;",response); } - else if(strcmp(cmd, "eraseTrace")==0) + else if(strcmp(cmd, "SendEraseTrace")==0) { num_point=0; - sprintf(response,"%seraseTrace;OK;",response); + sprintf(response,"%sSendEraseTrace;OK;",response); } else strcat(response,"Not understand\n"); diff --git a/owlps-ardrone/owlps-drone.h b/owlps-ardrone/owlps-drone.h index cae1621..bc32fdf 100644 --- a/owlps-ardrone/owlps-drone.h +++ b/owlps-ardrone/owlps-drone.h @@ -97,6 +97,12 @@ typedef struct _consigne struct _consigne *next; } consigne ; +typedef struct _point +{ + double lat; + double lon; + double alt; +} point_consigne ; /* Prototype drone */