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 */
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");

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

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