From 4ac4216ce0d351fb2f263fd99a8148ddbb891284 Mon Sep 17 00:00:00 2001 From: Florian Taillard Date: Tue, 31 May 2011 14:29:14 +0200 Subject: [PATCH] [ARDrone] Fix bug command Change measure angle to measure cap. Add flag 1 on command AT*PCMD. --- owlps-ardrone/owlps-drone.c | 173 ++++++++++++++++++++++-------------- owlps-ardrone/owlps-drone.h | 4 +- 2 files changed, 109 insertions(+), 68 deletions(-) diff --git a/owlps-ardrone/owlps-drone.c b/owlps-ardrone/owlps-drone.c index 56b56e1..27ec9ed 100644 --- a/owlps-ardrone/owlps-drone.c +++ b/owlps-ardrone/owlps-drone.c @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -25,7 +26,7 @@ #include #define SEPARATOR ',' #define EOL '$' -#define TYPE "WIFI" +#define TYPE "GPS" /* Options */ @@ -98,7 +99,7 @@ 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) ; @@ -116,13 +117,19 @@ int main(int argc, char *argv[]) memset(&share_relatif_cons,0,sizeof(share_relatif_cons)); memset(&share_relatif_retro,0,sizeof(share_relatif_retro)); + //Set unknow value gps share_gps_retro.lat=-999.00; share_gps_retro.lon=-999.00; + share_gps_cons.lat = -999.00; + share_gps_cons.lon = -999.00; + // Thread communication TCP OwlSIG pthread_t control ; pthread_create(&control, NULL, thread_control, NULL) ; + + //Launch thread or wifi depend of TYPE if(!strcmp(TYPE,"WIFI")) { pthread_t send ; @@ -143,17 +150,22 @@ int main(int argc, char *argv[]) return -1; } - //drone_socket_create() ; - //pthread_t drone_watchdog ; - //pthread_create(&drone_watchdog, NULL, drone_socket_watchdog, NULL) ; + drone_socket_create() ; + pthread_t drone_watchdog ; + pthread_create(&drone_watchdog, NULL, drone_socket_watchdog, NULL) ; + + log = fopen("vol.log", "w"); + 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(start==0) usleep(200000); + + //Wait GPS Signal if(share_gps.lat==0||share_gps.lon==0) printf("[GPS] GPS not fix! Wait...\n"); while(share_gps.lat==0||share_gps.lon==0); - log = fopen("vol.log", "w"); - fprintf(log, "Date;Heure;Latitude;Longitude;DistanceTotal;Distance;SpeedPitch;angle;x;y;x_retro;y_retro;x_cons;y_cons\n"); + /* Log File */ + /* - */ printf("[GPS] GPS fix OK!\n"); speed_pitch=-1; @@ -162,31 +174,34 @@ int main(int argc, char *argv[]) if(cmd_drone_take_off()<0) drone_err(2); - sleep(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)) { 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; - + //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; - cmd_drone_move(speed_pitch, angle); - sleep(1); - fprintf(log,"%d;%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, 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); + 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); } fclose(log); cmd_drone_landing(); @@ -224,7 +239,7 @@ void traficGPS(int gps) { do { c=read(gps, buffer, 4096); - printf("lecture intiale %d\n",c); + //printf("lecture intiale %d\n",c); } while (c>99); do @@ -266,7 +281,7 @@ void traficGPS(int gps) { } y++; } - printf("\nTaille finale : %d\n", length); + //printf("\nTaille finale : %d\n", length); y=0; while(y<=length) @@ -311,10 +326,11 @@ void traficGPS(int gps) { share_gps_retro.lon=share_gps.lon; share_gps.lat=datagps.latitude; share_gps.lon=datagps.longitude; + share_gps.cap=datagps.cap; } share_gps.date=datagps.date; share_gps.time=datagps.time; - printf("lat %s lon %s\n",data[3], data[5]); + //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); @@ -327,7 +343,6 @@ void* thread_gps(void* NULL_value) char buffer[1024]; int gps_fd = open("/dev/ttyPA0",O_RDONLY); - read(gps_fd, buffer, 80); traficGPS(gps_fd); pthread_exit(NULL_value); @@ -421,6 +436,11 @@ void* thread_control(void* NULL_value) cmd_drone_landing(); sprintf(response,"%sLanding;OK;",response); } + else if(strcmp(cmd, "eraseTrace")==0) + { + num_point=0; + sprintf(response,"%seraseTrace;OK;",response); + } else strcat(response,"Not understand\n"); n = write(newsockfd,response,strlen(response)); @@ -841,7 +861,7 @@ void string2data(char* string_data) } } results[count_algo].z = atof(ptr) ; - if(error==1) + if(error==1) { //Lecture du message Error ptr = strtok(NULL, delims) ; @@ -992,19 +1012,28 @@ void calcul_trajectory(float *distance, float *angle) /* T T | F */ /* ********** */ - if((a<0)!=(b<0)) - (*angle)=(*angle); - else (*angle) = -(*angle); + if(a*b>=0) (*angle) = -(*angle); } int check_destination(struct point drone, struct point target) { - if(oc_distance_between(drone, target)<1.8) + if(oc_distance_between(drone, target)<3) return 1; else return 0; } +float calcul_angle() +{ + float captarget = oc_cap(share_gps, share_gps_cons); + + if((share_gps.cap-captarget-180)<0) + return ((int)(360 - share_gps.cap + (captarget-180))%360)-180; + + else + return ((int)(360 - share_gps.cap + (180 - captarget))%360)-180; +} + int read_position(char *type) { if(!strcmp(type,"WIFI")) printf("Localisation by WIFI\n"); @@ -1020,6 +1049,7 @@ int read_position(char *type) } usleep(100000); } + share_relatif = oc_convert(share_gps_origin, share_gps); if(share_gps_retro.lat!=-999.00&&share_gps_retro.lon!=-999.00) share_relatif_retro = oc_convert(share_gps_origin, share_gps_retro); @@ -1059,36 +1089,38 @@ int cmd_drone_init_direction(void) sem_wait(&lock); //for(boucle=0;boucle<3;boucle++) while(share_gps_retro.lat==-999.00||share_gps_retro.lon==-999.00) - { + { int *floatint_speed = (int32_t*) &speed ; - sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,0\r", i, *floatint_speed); + sprintf(sendline,"AT*PCMD=%d,1,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); + sprintf(sendline,"AT*PCMD=%d,1,0,0,0,0\r", i); sendto(sockcmdfd,sendline,strlen(sendline),0, (struct sockaddr *)&servcmd,sizeof(servcmd)); sem_post(&lock); return 1; } -int cmd_drone_move(float speed_pitch, float angle) +int cmd_drone_move(float speed_pitch, int angle) { float speed_yaw; - if(state_flying==0) - return -1; - else - { - speed_yaw=(1/180)*angle; - sem_wait(&lock); - int *floatint_speed_pitch = (int32_t*) &speed_pitch; - int *floatint_speed_yaw = (int32_t*) &speed_yaw; - sprintf(sendline,"AT*PCMD=%d,0,0,%d,0,%d\r",i,*floatint_speed_pitch,*floatint_speed_yaw); - i++; - sendto(sockcmdfd,sendline,strlen(sendline),0, - (struct sockaddr *)&servcmd,sizeof(servcmd)); - } + //if(state_flying==0) + //return -1; + //else + //{ + speed_yaw=((float)angle/180); + sem_wait(&lock); + int *floatint_speed_pitch = (int32_t*) &speed_pitch; + int *floatint_speed_yaw = (int32_t*) &speed_yaw; + sprintf(sendline,"AT*PCMD=%d,1,0,%d,0,%d\r",i,*floatint_speed_pitch,*floatint_speed_yaw); + //printf("%s\n", sendline); + i++; + sendto(sockcmdfd,sendline,strlen(sendline),0, + (struct sockaddr *)&servcmd,sizeof(servcmd)); + //printf("[ATPCMD] pitch : %f yaw : %f\n", speed_pitch, speed_yaw); + //} sem_post(&lock); return 0; } @@ -1136,14 +1168,18 @@ int drone_socket_close(void) void* drone_socket_watchdog(void* NULL_value) { + struct timeval start, end; + long max, calc; + max = 0; while(run) { - usleep(50000); + gettimeofday(&start, NULL); + usleep(8000); memset(sendline, 0, sizeof(sendline)); if(i==-1) { - strncpy(sendline,"AT*CONFIG=1,\"general:navdata_demo\",\"TRUE\"\rAT*CONFIG=2,\"control:control_yaw\",\"2.5\"\rAT*PMODE=3,2\rAT*MISC=4,2,20,2000,3000\r", LENSTR); - i=5; + strncpy(sendline,"AT*CONFIG=1,\"control:outdoor\",\"TRUE\"\rAT*CONFIG=2,\"general:navdata_demo\",\"TRUE\"\rAT*CONFIG=3,\"control:control_yaw\",\"2.5\"\rAT*PMODE=4,2\rAT*MISC=5,2,20,2000,3000\r", LENSTR); + i=6; } else { @@ -1170,7 +1206,12 @@ void* drone_socket_watchdog(void* NULL_value) } sendto(sockcmdfd,sendline,strlen(sendline),0, (struct sockaddr *)&servcmd,sizeof(servcmd)); + //printf("[WATCHDOG] %s\n", sendline); sem_post(&lock); + gettimeofday(&end, NULL); + + calc = ((end.tv_sec * 1000000 + end.tv_usec) - (start.tv_sec * 1000000 + start.tv_usec)); + //printf("[WATCHDOG] Time usec : %ld\n", calc); } pthread_exit(NULL_value); } @@ -1193,40 +1234,40 @@ void drone_handler_sigint(const int num) consigne* list_add_front(consigne* list, double latitude, double longitude, double altitude) { - consigne* new_consigne = malloc(sizeof(consigne)); + consigne* new_consigne = malloc(sizeof(consigne)); - new_consigne->lat = latitude; - new_consigne->lon = longitude; - new_consigne->alt = altitude; + new_consigne->lat = latitude; + new_consigne->lon = longitude; + new_consigne->alt = altitude; - new_consigne->next = list; + new_consigne->next = list; - return new_consigne; + return new_consigne; } consigne* list_add_back(consigne* list, double lat, double lon, double alt) { - consigne* new_consigne = malloc(sizeof(consigne)); + consigne* new_consigne = malloc(sizeof(consigne)); - new_consigne->lat = lat; - new_consigne->lon = lon; - new_consigne->alt = alt; + new_consigne->lat = lat; + new_consigne->lon = lon; + new_consigne->alt = alt; - new_consigne->next = NULL; + new_consigne->next = NULL; - if(list == NULL) + if(list == NULL) { - return new_consigne; + return new_consigne; } - else + else { - consigne* tmp=list; - while(tmp->next != NULL) + consigne* tmp=list; + while(tmp->next != NULL) { - tmp = tmp->next; + tmp = tmp->next; } - tmp->next = new_consigne; - return list; + tmp->next = new_consigne; + return list; } } diff --git a/owlps-ardrone/owlps-drone.h b/owlps-ardrone/owlps-drone.h index 352c277..cae1621 100644 --- a/owlps-ardrone/owlps-drone.h +++ b/owlps-ardrone/owlps-drone.h @@ -110,13 +110,13 @@ int drone_socket_close(void); //Commands drone int cmd_drone_take_off(void); //Add socket int cmd_drone_landing(void); //Add socket -int cmd_drone_move(float, float); +int cmd_drone_move(float, int); int cmd_drone_init_direction(void); //Add socket //Algo void calcul_trajectory(float*, float*); float calcul_speed(float); int check_destination(struct point, struct point); - +float calcul_angle(void); //Liste chaine consigne* list_add_front(consigne*, double, double, double); consigne* list_add_back(consigne*, double, double, double);