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

[ARDrone] Add fly log file 

Save date, time, latitude, longitude, speed_pitch and angle in file
vol.log for debugging.
This commit is contained in:
Florian Taillard 2011-05-09 13:54:34 +02:00 committed by Matteo Cypriani
parent 10e38bfc0b
commit 3ba38df483
1 changed files with 53 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -93,13 +93,13 @@ double share_t_cons=-999.00;
int main(int argc, char *argv[])
{
FILE* log = NULL;
float angle, distance, speed_pitch, distance_total;
struct sigaction action;
program_name = argv[0] ;
parse_command_line(argc, argv) ;
action.sa_flags = 0;
sigemptyset(&action.sa_mask);
action.sa_handler = drone_handler_sigint;
@ -141,39 +141,49 @@ int main(int argc, char *argv[])
drone_socket_create() ;
pthread_t drone_watchdog ;
pthread_create(&drone_watchdog, NULL, drone_socket_watchdog, NULL) ;
while(start==0) usleep(200000);
speed_pitch=-1;
if(read_position(TYPE)<0)
drone_err(1);
if(cmd_drone_take_off()<0)
drone_err(2);
if(cmd_drone_init_direction()<0)
drone_err(3);
calcul_trajectory(&angle, &distance);
distance_total = distance;
while(!check_destination(share_gps, share_gps_cons))
while(run)
{
while(start==0) usleep(200000);
while(share_gps.lat==0||share_gps.lon==0);
log = fopen("vol.log", "w");
fprintf(log, "Date;Heure;Latitude;Longitude;DistanceTotal;Distance;SpeedPitch;angle\n");
printf("[GPS] GPS fix OK!\n");
speed_pitch=-1;
if(read_position(TYPE)<0)
drone_err(1);
calcul_trajectory(&angle, &distance);
if(cmd_drone_take_off()<0)
drone_err(2);
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));
if(speed_pitch<-(distance/distance_total))
speed_pitch=speed_pitch;
else
speed_pitch=-(distance/distance_total);
if(cmd_drone_init_direction()<0)
drone_err(3);
cmd_drone_move(speed_pitch, angle);
sleep(1);
while(start==1&&!check_destination(share_gps, share_gps_cons))
{
if(read_position(TYPE)<0)
drone_err(1);
calcul_trajectory(&angle, &distance);
if(speed_pitch < -1) speed_pitch = -1;
else if(speed_pitch > 1) speed_pitch = 1;
if(speed_pitch<-(distance/distance_total))
speed_pitch=speed_pitch;
else
speed_pitch=-(distance/distance_total);
cmd_drone_move(speed_pitch, angle);
sleep(1);
fprintf(log,"%d;%f;%f;%f;%f;%f;%f;%f\n",share_gps.date, share_gps.time, share_gps.lat, share_gps.lon, distance_total, distance, speed_pitch, angle);
printf("[DEBUG] Vitesse %f Angle %f\n",speed_pitch, angle);
}
fclose(log);
cmd_drone_landing();
}
(void) close(socksendfd) ;
(void) close(sockreceivefd) ;
@ -291,9 +301,11 @@ void traficGPS(int gps) {
share_gps_retro.lon=share_gps.lon;
share_gps.lat=datagps.latitude;
share_gps.lon=datagps.longitude;
share_gps.date=datagps.date;
share_gps.time=datagps.time;
printf("lat %s lon %s\n",data[3], data[5]);
printf("Time : %f | State : %c | Latitude : %.10f%c | Longitude %.10f%c | Speed : %f km/h | Cap : %f° | Date : %d | DecMagn : %f maps %.10f %.10f\n",datagps.time, datagps.state, datagps.latitude, datagps.orilat, datagps.longitude, datagps.orilon, datagps.speed, datagps.cap, datagps.date, datagps.decmag, share_gps.lat, share_gps.lon);
//printf("Time : %f | State : %c | Latitude : %.10f%c | Longitude %.10f%c | Speed : %f km/h | Cap : %f° | Date : %d | DecMagn : %f maps %.10f %.10f\n",datagps.time, datagps.state, datagps.latitude, datagps.orilat, datagps.longitude, datagps.orilon, datagps.speed, datagps.cap, datagps.date, datagps.decmag, share_gps.lat, share_gps.lon);
}
}
@ -351,17 +363,17 @@ void* thread_control(void* NULL_value)
perror("ERROR on accept");
// while ( strcmp(request,"end")!=0) {
n = read(newsockfd,request,1500);
printf("\ntaille %d\n", n);
//printf("\ntaille %d\n", n);
fflush(NULL);
if (n < 0) perror("ERROR reading from socket");
printf("%s", request);
//printf("%s", request);
// Add command processing
c=strtok(request,"=");
printf("val 1 %s\n",c);
//printf("val 1 %s\n",c);
c=strtok(NULL,"=");
printf("val 2 %s\n",c);
//printf("val 2 %s\n",c);
cmd=strtok(c,";");
printf("val 3 %s\n",cmd);
//printf("val 3 %s\n",cmd);
sx=strtok(NULL,";");
sy=strtok(NULL,";");
sz=strtok(NULL,";");
@ -378,6 +390,7 @@ void* thread_control(void* NULL_value)
share_gps_cons.alt=atof(sz);
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);
}
else if (strcmp(cmd, "SendSetpoint")==0) {
share_relatif_cons.x=atof(sx);
@ -394,7 +407,7 @@ void* thread_control(void* NULL_value)
else if(strcmp(cmd, "SendLanding")==0)
{
start=0;
cmd_drone_landing();
//cmd_drone_landing();
sprintf(response,"%sLanding;OK;",response);
}
else
@ -1000,7 +1013,7 @@ int read_position(char *type)
share_relatif_retro = oc_convert(share_gps_origin, share_gps_retro);
share_relatif_cons = oc_convert(share_gps_origin, share_gps_cons);
printf("X current : %f Y current : %f X old : %f Y old : %f X target : %f Y target : %f\n",share_relatif.x, share_relatif.y, -share_relatif_retro.x, share_relatif_retro.y, share_relatif_cons.x, share_relatif_cons.y);
//printf("X current : %f Y current : %f X old : %f Y old : %f X target : %f Y target : %f\n",share_relatif.x, share_relatif.y, -share_relatif_retro.x, share_relatif_retro.y, share_relatif_cons.x, share_relatif_cons.y);
}
else return -1;
@ -1037,7 +1050,13 @@ int cmd_drone_init_direction(void)
{
int *floatint_speed = (int32_t*) &speed ;
sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,0\r", i, *floatint_speed);
sendto(sockcmdfd,sendline,strlen(sendline),0,
(struct sockaddr *)&servcmd,sizeof(servcmd));
usleep(100000);
}
sprintf(sendline,"AT*PCMD=%d,0,0,0,0,0\r", i);
sendto(sockcmdfd,sendline,strlen(sendline),0,
(struct sockaddr *)&servcmd,sizeof(servcmd));
lock=0;
return 1;
}
@ -1058,7 +1077,6 @@ int cmd_drone_move(float speed_pitch, float angle)
i++;
sendto(sockcmdfd,sendline,strlen(sendline),0,
(struct sockaddr *)&servcmd,sizeof(servcmd));
}
lock=0;
return 0;