diff --git a/owlps-ardrone/owlps-drone.c b/owlps-ardrone/owlps-drone.c index 473dc33..3d0e01c 100644 --- a/owlps-ardrone/owlps-drone.c +++ b/owlps-ardrone/owlps-drone.c @@ -70,14 +70,10 @@ owl_bool is_calibration_request = FALSE ; int socksendfd ; // Sending socket descriptor (send positioning packets) int sockreceivefd ; // Receiving socket descriptor (Receive position) -int socksendlvfd ; // Sending labview socket descriptor (Send position to labview) struct sockaddr_in server ; // Server info uint8_t *packet = NULL ; // Packet to send uint_fast16_t packet_size ; // Packet size -// Shared variables for OwlSIG (Geo Information System) -//double share_x, share_y, share_z; - struct point share_gps; struct point share_gps_retro; consigne* list_share_gps_cons = NULL; @@ -161,7 +157,10 @@ int main(int argc, char *argv[]) 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"); + 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; @@ -175,10 +174,10 @@ int main(int argc, char *argv[]) /* - */ printf("[GPS] GPS fix OK!\n"); speed_pitch=-1; - + if(read_position(TYPE)<0) drone_err(1); - + if(cmd_drone_take_off()<0) drone_err(2); while(share_gps_cons.lat==-999.00||share_gps_cons.lon==-999.00); @@ -189,26 +188,45 @@ int main(int argc, char *argv[]) share_gps_cons.lon = array_cons[i_consigne].lon; share_gps_cons.alt = array_cons[i_consigne].alt; - distance_total=fabs(oc_distance_between(share_gps, share_gps_cons)); - + 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)); + distance = + fabs(oc_distance_between(share_gps, share_gps_cons)) ; if(speed_pitch<-(distance/distance_total)) speed_pitch=-(distance/distance_total)-0.2; 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); + 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++; @@ -217,9 +235,9 @@ int main(int argc, char *argv[]) cmd_drone_landing(); } sem_destroy(&lock); - (void) close(socksendfd) ; - (void) close(sockreceivefd) ; - + close(socksendfd) ; + close(sockreceivefd) ; + return 0 ; } @@ -245,18 +263,18 @@ void parse_command_line(int argc, char **argv) void traficGPS(int gps) { int offset=0, c=1, i=0; char buffer[4096]; - - do + + do { c=read(gps, buffer, 4096); //printf("lecture intiale %d\n",c); - } + } while (c>99); - do + do { c=read(gps, buffer+offset, 1); offset=offset+c; - } + } while (buffer[offset-c]!='$'); //buffer[offset]='\0'; //printf("phrase x%sx\n",buffer); @@ -264,11 +282,11 @@ void traficGPS(int gps) { while (run) { i=0; offset=0; - do + do { c=read(gps, buffer+offset, 1); offset=offset+c; - } + } while (buffer[offset-c]!='$'); //buffer[offset]='\0'; //printf("phrase s x%sx\n",buffer); @@ -304,7 +322,8 @@ void traficGPS(int gps) { else { - memcpy(data[ct_string], buffer+i+offset_line, y-offset_line); + memcpy(data[ct_string], buffer + i + offset_line, + y - offset_line) ; data[ct_string][y-offset_line]=0; } ct_string++; @@ -314,14 +333,19 @@ void traficGPS(int gps) { } if(!(ct_string==0 || offset_line==0)) - memcpy(data[ct_string], buffer+i+offset_line, length-offset_line); + 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.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.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]); @@ -348,6 +372,7 @@ void traficGPS(int gps) { } } + void* thread_gps(void* NULL_value) { char buffer[1024]; @@ -358,6 +383,7 @@ void* thread_gps(void* NULL_value) pthread_exit(NULL_value); } + void* thread_send(void* NULL_value) { while(run) @@ -369,6 +395,7 @@ void* thread_send(void* NULL_value) pthread_exit(NULL_value); } + // TCP Server OwlSIG void* thread_control(void* NULL_value) { @@ -415,18 +442,30 @@ void* thread_control(void* NULL_value) 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"); + strcpy(response, + "HTTP/1.1 200 OK\n" + "Date: Mon, 18 Apr 2011 19:51:52 GMT\n" + "Server: Apache/ProXad [Aug 5 2010 16:17:11]\n" + "Access-Control-Allow-Origin: *\n" + "X-Powered-By: PHP/4.4.3-dev\n" + "Connection: close\n" + "Content-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_gps.lat,share_gps.lon,share_gps.alt); + sprintf(response, "%sGeoPosition;OK;%f;%f;%f\n", + response, share_gps.lat, share_gps. lon,share_gps.alt) ; else if (strcmp(cmd, "SendGeoSetpoint")==0) { share_gps_cons.lat=atof(sx); share_gps_cons.lon=atof(sy); 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); + 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; @@ -438,7 +477,9 @@ void* thread_control(void* NULL_value) share_relatif_cons.y=atof(sy); share_relatif_cons.z=atof(sz); share_t_cons=atof(st); - sprintf(response,"%sSetpoint;OK;%f;%f;%f;%f\n",response,share_relatif_cons.x,share_relatif_cons.y,share_relatif_cons.z,share_t_cons); + sprintf(response, "%sSetpoint;OK;%f;%f;%f;%f\n", + response, share_relatif_cons.x, share_relatif_cons.y, + share_relatif_cons.z, share_t_cons) ; } else if(strcmp(cmd, "SendStart")==0) { @@ -469,6 +510,7 @@ void* thread_control(void* NULL_value) pthread_exit(NULL_value); } + void parse_main_options(int argc, char **argv) { int opt ; @@ -755,6 +797,7 @@ void* receive_position(void* NULL_value) pthread_exit(NULL_value); } + void string2data(char* string_data) { /*Découpage de la chaine de caractère reçu du serveur de positionnement @@ -923,27 +966,38 @@ void string2data(char* string_data) count_algo++; } - for(count_print=0;count_printlat = latitude; new_consigne->lon = longitude; new_consigne->alt = altitude; new_consigne->next = list; - + return new_consigne; } -consigne* list_add_back(consigne* list, double lat, double lon, double alt) + +consigne* list_add_back(consigne* list, double lat, + double lon, double alt) { - + consigne* new_consigne = malloc(sizeof(consigne)); - + new_consigne->lat = lat; new_consigne->lon = lon; new_consigne->alt = alt; - + new_consigne->next = NULL; - + if(list == NULL) - { - return new_consigne; - } + return new_consigne ; else { consigne* tmp=list;