mcy
/
owlps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[ARDrone] Add support for multi-point of consigne

This commit is contained in:
Florian Taillard 2011-06-14 14:04:50 +02:00 committed by Matteo Cypriani
parent 82e65d375d
commit 5126894634
2 changed files with 45 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
owlps-ardrone/owlps-drone.c
View File

@ -31,6 +31,10 @@
/* Options */ /* Options */
gps datagps; gps datagps;
point_consigne array_cons[30];
int i_consigne=0;
int num_point=0;
struct timespec timestamp ; struct timespec timestamp ;
struct { struct {
@ -99,7 +103,6 @@ int main(int argc, char *argv[])
FILE* log = NULL; FILE* log = NULL;
float angle, distance, speed_pitch, distance_total; float angle, distance, speed_pitch, distance_total;
struct sigaction action; struct sigaction action;
int boot=0;
program_name = argv[0] ; program_name = argv[0] ;
parse_command_line(argc, argv) ; 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"); 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) while(run)
{ {
i_consigne=0;
while(start==0) usleep(200000); while(start==0) usleep(200000);
//Wait GPS Signal //Wait GPS Signal
@ -176,32 +180,35 @@ int main(int argc, char *argv[])
drone_err(2); drone_err(2);
while(share_gps_cons.lat==-999.00||share_gps_cons.lon==-999.00); while(share_gps_cons.lat==-999.00||share_gps_cons.lon==-999.00);
sleep(8); sleep(8);
distance_total=fabs(oc_distance_between(share_gps, share_gps_cons)); while(i_consigne<=num_point)
if(cmd_drone_init_direction()<0)
drone_err(3);
boot=0;
while(start==1&&!check_destination(share_gps, share_gps_cons))
{ {
if(read_position(TYPE)<0) share_gps_cons.lat = array_cons[i_consigne].lat;
drone_err(1); share_gps_cons.lon = array_cons[i_consigne].lon;
share_gps_cons.alt = array_cons[i_consigne].alt;
//calcul_trajectory(&angle, &distance_old); distance_total=fabs(oc_distance_between(share_gps, share_gps_cons));
angle = calcul_angle();
distance = fabs(oc_distance_between(share_gps, share_gps_cons)); while(start==1&&!check_destination(share_gps, share_gps_cons))
if(speed_pitch<-(distance/distance_total)) {
speed_pitch=speed_pitch; if(read_position(TYPE)<0)
else drone_err(1);
speed_pitch=-(distance/distance_total);
if(speed_pitch < -1) speed_pitch = -1; //calcul_trajectory(&angle, &distance_old);
else if(speed_pitch > 1) speed_pitch = 1; 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)); if(speed_pitch < -1) speed_pitch = -1;
usleep(10000); else if(speed_pitch > 1) speed_pitch = 1;
//boot++;
printf("Vitesse : %f Angle : %f\n", (speed_pitch/5), angle/2); cmd_drone_move((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/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); usleep(10000);
//printf("[DEBUG] Vitesse %f Angle %f state %d\n",speed_pitch, angle,state_flying); 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); fclose(log);
cmd_drone_landing(); cmd_drone_landing();
@ -209,7 +216,7 @@ int main(int argc, char *argv[])
sem_destroy(&lock); sem_destroy(&lock);
(void) close(socksendfd) ; (void) close(socksendfd) ;
(void) close(sockreceivefd) ; (void) close(sockreceivefd) ;
return 0 ; return 0 ;
} }
@ -417,6 +424,11 @@ void* thread_control(void* NULL_value)
share_t_cons=atof(st); 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); 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); 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) { else if (strcmp(cmd, "SendSetpoint")==0) {
share_relatif_cons.x=atof(sx); share_relatif_cons.x=atof(sx);
@ -436,10 +448,10 @@ void* thread_control(void* NULL_value)
cmd_drone_landing(); cmd_drone_landing();
sprintf(response,"%sLanding;OK;",response); sprintf(response,"%sLanding;OK;",response);
} }
else if(strcmp(cmd, "eraseTrace")==0) else if(strcmp(cmd, "SendEraseTrace")==0)
{ {
num_point=0; num_point=0;
sprintf(response,"%seraseTrace;OK;",response); sprintf(response,"%sSendEraseTrace;OK;",response);
} }
else else
strcat(response,"Not understand\n"); strcat(response,"Not understand\n");

6
owlps-ardrone/owlps-drone.h
View File

@ -97,6 +97,12 @@ typedef struct _consigne
struct _consigne *next; struct _consigne *next;
} consigne ; } consigne ;
typedef struct _point
{
double lat;
double lon;
double alt;
} point_consigne ;
/* Prototype drone */ /* Prototype drone */