diff --git a/owlps-ardrone/owlps-drone.c b/owlps-ardrone/owlps-drone.c index e790808..3159b1d 100644 --- a/owlps-ardrone/owlps-drone.c +++ b/owlps-ardrone/owlps-drone.c @@ -64,8 +64,10 @@ uint8_t *packet = NULL ; // Packet to send uint_fast16_t packet_size ; // Packet size // Shared variables for OwlSIG (Geo Information System) -float share_x, share_y, share_z; +double share_x, share_y, share_z; double share_lat, share_lon, share_alt=0; +double share_lat_cons=-999.0, share_lon_cons=-999.00, share_alt_cons=-999.0; +double share_x_cons=-999.0, share_y_cons=-999.00, share_z_cons=-999.0, share_t_cons=-999.0; int main(int argc, char *argv[]) { @@ -84,7 +86,7 @@ int main(int argc, char *argv[]) { if (options.listening_port > 0) receive_position() ; - else usleep(1000); + else usleep(1000); } (void) close(socksendfd) ; (void) close(sockreceivefd) ; @@ -106,109 +108,119 @@ void parse_command_line(int argc, char **argv) #endif // DEBUG } -void traficGPS(int gps) { +void traficGPS(int gps) +{ int offset=0, c=1, i=0, l; char buffer[4096], chtime[1024]; char *utc, *latitude, *longitude, *chv, *cmd, *ch; float vitesse; - do { - c=read(gps, buffer, 4096); - printf("lecture intiale %d\n",c); - } while (c>99); - do { - c=read(gps, buffer+offset, 1); - offset=offset+c; - } while (buffer[offset-c]!='$'); + do + { + c=read(gps, buffer, 4096); + printf("lecture intiale %d\n",c); + } + while (c>99); + do + { + c=read(gps, buffer+offset, 1); + offset=offset+c; + } + while (buffer[offset-c]!='$'); //buffer[offset]='\0'; //printf("phrase x%sx\n",buffer); // Debut de phrase GPS sans le dollar while (1) { i=0; offset=0; - do { - c=read(gps, buffer+offset, 1); - offset=offset+c; - } while (buffer[offset-c]!='$'); + do + { + c=read(gps, buffer+offset, 1); + offset=offset+c; + } + while (buffer[offset-c]!='$'); //buffer[offset]='\0'; //printf("phrase s x%sx\n",buffer); if (strncmp(buffer+i,"GPRMC",5)==0) - { - char data[15][20]; - int offset_line, length, ct_string, y; - int z; + { + char data[15][20]; + int offset_line, length, ct_string, y; + int z; - ct_string = offset_line = y = z = 0 ; - length = strlen(buffer+i) ; - memset(data,0,200); - while(y<=length) - { - if((buffer)[y+i]==EOL && z==0) z=1; - else if((buffer)[y+i]==EOL && z==1) - { - length=y-1; - break; - } - y++; - } - printf("\nTaille finale : %d\n", length); + ct_string = offset_line = y = z = 0 ; + length = strlen(buffer+i) ; + memset(data,0,200); + while(y<=length) + { + if((buffer)[y+i]==EOL && z==0) z=1; + else if((buffer)[y+i]==EOL && z==1) + { + length=y-1; + break; + } + y++; + } + printf("\nTaille finale : %d\n", length); - y=0; - while(y<=length) - { - if ((buffer)[y+i]==SEPARATOR) - { + y=0; + while(y<=length) + { + if ((buffer)[y+i]==SEPARATOR) + { - if((y-offset_line)==0) - memcpy(data[ct_string], "\0", 1); + if((y-offset_line)==0) + memcpy(data[ct_string], "\0", 1); - else - { - memcpy(data[ct_string], buffer+i+offset_line, y-offset_line); - data[ct_string][y-offset_line]=0; - } - ct_string++; - offset_line=y+1; - } - y++; - } + else + { + memcpy(data[ct_string], buffer+i+offset_line, y-offset_line); + data[ct_string][y-offset_line]=0; + } + ct_string++; + offset_line=y+1; + } + y++; + } - if(!(ct_string==0 || offset_line==0)) - memcpy(data[ct_string], buffer+i+offset_line, length-offset_line); - data[ct_string][length-offset_line]=0; + if(!(ct_string==0 || offset_line==0)) + memcpy(data[ct_string], buffer+i+offset_line, length-offset_line); + data[ct_string][length-offset_line]=0; - datagps.time = atof(data[1]); - datagps.state = data[2][0]; - datagps.latitude = atof(strndup(data[3],2))+atof(strndup(data[3]+2,strlen(data[3])-2))/60.0 ; - datagps.orilat = data[4][0]; - datagps.longitude = atof(strndup(data[5],3))+atof(strndup(data[5]+3,strlen(data[5])-3))/60.0 ; - datagps.orilon = data[6][0]; - datagps.speed = atof(data[7])*1.852; - datagps.cap = atof(data[8]); - datagps.date = atoi(data[9]); - datagps.decmag = atof(data[10]); + datagps.time = atof(data[1]); + datagps.state = data[2][0]; + datagps.latitude = atof(strndup(data[3],2))+atof(strndup(data[3]+2,strlen(data[3])-2))/60.0 ; + datagps.orilat = data[4][0]; + datagps.longitude = atof(strndup(data[5],3))+atof(strndup(data[5]+3,strlen(data[5])-3))/60.0 ; + datagps.orilon = data[6][0]; + datagps.speed = atof(data[7])*1.852; + datagps.cap = atof(data[8]); + datagps.date = atoi(data[9]); + datagps.decmag = atof(data[10]); - share_lat=datagps.latitude; - share_lon=datagps.longitude; - printf("Time : %f | State : %c | Latitude : %f%c | Longitude %f%c | Speed : %f km/h | Cap : %f° | Date : %d | DecMagn : %f \n",datagps.time, datagps.state, datagps.latitude, datagps.orilat, datagps.longitude, datagps.orilon, datagps.speed, datagps.cap, datagps.date, datagps.decmag); + if (datagps.orilat=='S') datagps.latitude=-datagps.latitude; + if (datagps.orilon=='W') datagps.longitude=-datagps.longitude; + share_lat=datagps.latitude; + share_lon=datagps.longitude; + 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_lat, share_lon); - /* printf("Taille : %d | Message : %s\n",strlen(buffer+i),buffer+i); - cmd=strtok(buffer+i,","); - utc=strtok(NULL,","); - ch=strtok(NULL,","); - latitude=strtok(NULL,","); - ch=strtok(NULL,","); - longitude=strtok(NULL,","); - ch=strtok(NULL,","); - chv=strtok(NULL,","); - //printf("h %s lt %s lg %s v %s\n",utc, latitude, longitude,chv); - vitesse=atof(chv)*1.852; - share_lat=atof(strndup(latitude,2))+atof(strndup(latitude+2,strlen(latitude)-2))/60.0; - share_lon=atof(strndup(longitude,3))+atof(strndup(longitude+3,strlen(longitude)-3))/60.0; - printf("h %s lt %s lg %s v %f km/h share lat lon %f %f\n",utc, latitude, longitude,vitesse, share_lat, share_lon); - */ - } + /* printf("Taille : %d | Message : %s\n",strlen(buffer+i),buffer+i); + cmd=strtok(buffer+i,","); + utc=strtok(NULL,","); + ch=strtok(NULL,","); + latitude=strtok(NULL,","); + ch=strtok(NULL,","); + longitude=strtok(NULL,","); + ch=strtok(NULL,","); + chv=strtok(NULL,","); + //printf("h %s lt %s lg %s v %s\n",utc, latitude, longitude,chv); + vitesse=atof(chv)*1.852; + share_lat=atof(strndup(latitude,2))+atof(strndup(latitude+2,strlen(latitude)-2))/60.0; + share_lon=atof(strndup(longitude,3))+atof(strndup(longitude+3,strlen(longitude)-3))/60.0; + printf("h %s lt %s lg %s v %f km/h share lat lon %f %f\n",utc, latitude, longitude,vitesse, share_lat, share_lon); + */ + } } } @@ -230,6 +242,7 @@ void* thread_send(void* data) { make_packet() ; send_request() ; + sleep(1) ; } return NULL; } @@ -237,109 +250,79 @@ void* thread_send(void* data) // TCP Server OwlSIG void* thread_control(void* data) { -<<<<<<< HEAD int sockfd, newsockfd, portno=8080; socklen_t clilen; - char request[1024], response[1204]; + char request[2048], response[2048], *c; struct sockaddr_in serv_addr, cli_addr; - int n; + int n,i=4; + char *cmd, *sx, *sy, *sz, *st; sockfd = socket(AF_INET, SOCK_STREAM, 0); if (sockfd < 0) - error("ERROR opening socket TCP"); + perror("ERROR opening socket TCP"); bzero((char *) &serv_addr, sizeof(serv_addr)); serv_addr.sin_family = AF_INET; serv_addr.sin_addr.s_addr = INADDR_ANY; serv_addr.sin_port = htons(portno); if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) - error("ERROR on binding"); + perror("ERROR on binding"); listen(sockfd,1); clilen = sizeof(cli_addr); - while (1) - { - newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen); - if (newsockfd < 0) - error("ERROR on accept"); - bzero(request,1024); - while ( strcmp(request,"end")!=0) { - n = read(newsockfd,request,1024); - if (n < 0) perror("ERROR reading from socket"); - printf("%s", request); - // Add command processing - sprintf(response,"Position;OK;1.2;3.4;0.0"); - //sprintf(response,"Position;OK;%f;%f;%f",share_x, share_y, share_z); - n = write(newsockfd,response,strlen(response)); - if (n < 0) perror("ERROR writing to socket"); - } - close(newsockfd); - close(sockfd); + memset(request,0,1500); + cmd=malloc(256); + strcpy(cmd,""); + while ( strcmp(cmd,"end")!=0) { + newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen); + if (newsockfd < 0) + perror("ERROR on accept"); + // while ( strcmp(request,"end")!=0) { + n = read(newsockfd,request,1500); + printf("\ntaille %d\n", n); + fflush(NULL); + if (n < 0) perror("ERROR reading from socket"); + printf("%s", request); + // Add command processing + c=strtok(request,"="); + printf("val 1 %s\n",c); + c=strtok(NULL,"="); + printf("val 2 %s\n",c); + cmd=strtok(c,";"); + printf("val 3 %s\n",cmd); + sx=strtok(NULL,";"); + sy=strtok(NULL,";"); + sz=strtok(NULL,";"); + st=strtok(NULL,";"); + + strcpy(response,"HTTP/1.1 200 OK\nDate: Mon, 18 Apr 2011 19:51:52 GMT\nServer: Apache/ProXad [Aug 5 2010 16:17:11]\nAccess-Control-Allow-Origin: *\nX-Powered-By: PHP/4.4.3-dev\nConnection: close\nContent-Type: text/html\n\n"); + if (strcmp(cmd, "ReadPosition")==0) + sprintf(response,"%sPosition;OK;%d.0;5.0;6.1\n",response,i++); + else if (strcmp(cmd, "ReadGeoPosition")==0) + sprintf(response,"%sGeoPosition;OK;%f;%f;%f\n",response,share_lat,share_lon,share_alt); + else if (strcmp(cmd, "SendGeoSetpoint")==0) { + share_lat_cons=atof(sx); + share_lon_cons=atof(sy); + share_alt_cons=atof(sz); + share_t_cons=atof(st); + sprintf(response,"%sGeoSetpoint;OK;%.15f;%.15f;%f;%f\n",response,share_lat_cons,share_lon_cons,share_alt_cons,share_t_cons); } + else if (strcmp(cmd, "SendSetpoint")==0) { + share_x_cons=atof(sx); + share_y_cons=atof(sy); + share_z_cons=atof(sz); + share_t_cons=atof(st); + sprintf(response,"%sSetpoint;OK;%f;%f;%f;%f\n",response,share_x_cons,share_y_cons,share_z_cons,share_t_cons); + } + else + strcat(response,"Not understand\n"); + n = write(newsockfd,response,strlen(response)); + if (n < 0) perror("ERROR writing to socket"); + printf ("Write : %d %d\n", n, (int) strlen(response)); + close(newsockfd); + + // } + } + close(sockfd); return NULL; -======= - int sockfd, newsockfd, portno=8080; - socklen_t clilen; - char request[2048], response[2048], *c; - struct sockaddr_in serv_addr, cli_addr; - int n,i=4; - char *cmd, *sx, *sy, *sz, *st; - - sockfd = socket(AF_INET, SOCK_STREAM, 0); - if (sockfd < 0) - perror("ERROR opening socket TCP"); - bzero((char *) &serv_addr, sizeof(serv_addr)); - serv_addr.sin_family = AF_INET; - serv_addr.sin_addr.s_addr = INADDR_ANY; - serv_addr.sin_port = htons(portno); - if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) - perror("ERROR on binding"); - listen(sockfd,1); - clilen = sizeof(cli_addr); - memset(request,0,1500); - cmd=malloc(256); - strcpy(cmd,""); - while ( strcmp(cmd,"end")!=0) { - newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen); - if (newsockfd < 0) - perror("ERROR on accept"); -// while ( strcmp(request,"end")!=0) { - n = read(newsockfd,request,1500); - printf("\ntaille %d\n", n); - fflush(NULL); - if (n < 0) perror("ERROR reading from socket"); - printf("%s", request); - // Add command processing - c=strtok(request,"="); - printf("val 1 %s\n",c); - c=strtok(NULL,"="); - printf("val 2 %s\n",c); - cmd=strtok(c,";"); - printf("val 3 %s\n",cmd); - sx=strtok(NULL,";"); - sy=strtok(NULL,";"); - sz=strtok(NULL,";"); - st=strtok(NULL,";"); - -strcpy(response,"HTTP/1.1 200 OK\nDate: Mon, 18 Apr 2011 19:51:52 GMT\nServer: Apache/ProXad [Aug 5 2010 16:17:11]\nAccess-Control-Allow-Origin: *\nX-Powered-By: PHP/4.4.3-dev\nConnection: close\nContent-Type: text/html\n\n"); - if (strcmp(cmd, "ReadPosition")==0) - sprintf(response,"%sPosition;OK;%d.0;5.0;6.1\n",response,i++); - else if (strcmp(cmd, "ReadGeoPosition")==0) - sprintf(response,"%sGeoPosition;OK;%f;%f;%f\n",response,share_lat,share_lon,share_alt); - else if (strcmp(cmd, "SendGeoSetpoint")==0) - sprintf(response,"%sGeoSetpoint;OK;%d.0;5.0;6.1;0.0\n",response,i++); - else if (strcmp(cmd, "SendSetpoint")==0) - sprintf(response,"%sSetpoint;OK;%s;%s;%s;%s\n",response,sx,sy,sz,st); - else - strcat(response,"Not understand\n"); - n = write(newsockfd,response,strlen(response)); - if (n < 0) perror("ERROR writing to socket"); - printf ("Write : %d %d\n", n, (int) strlen(response)); - close(newsockfd); - -// } - } - close(sockfd); - return NULL; ->>>>>>> ea33a1a... [ARDrone] Add support GPS NMEA } void parse_main_options(int argc, char **argv)