diff --git a/owlps-ardrone/owlps-drone.c b/owlps-ardrone/owlps-drone.c index 4d4b482..8c7a65d 100644 --- a/owlps-ardrone/owlps-drone.c +++ b/owlps-ardrone/owlps-drone.c @@ -16,40 +16,6 @@ #include #include -#define DEBUG - -/* Error codes */ -#define ERR_BAD_USAGE 1 // Bad program call (bad number of arguments) - -/* Number of packets to send */ -#define DEFAULT_NBPKT_CALIB 20 // 20 packets when calibrating -#define DEFAULT_NBPKT_NORMAL 10 // 10 packets when requesting the position - -/* Delay between two packet transmissions (in microseconds) */ -#define DEFAULT_DELAY_CALIB 50000 // Calibration request -#define DEFAULT_DELAY_NORMAL 25000 // Localisation request - -/* Program arguments (getopt string) */ -#define OPTIONS "d:hi:l::n:p:t:" - - -/* Function headers */ -void parse_command_line(int argc, char **argv) ; -void parse_main_options(int argc, char **argv) ; -void check_destination_ip(void) ; -void parse_calibration_data(int argc, char **argv) ; -void check_configuration(void) ; -#ifdef DEBUG -void print_configuration(void) ; -#endif // DEBUG -void create_socket(void) ; -void make_packet(void) ; -void send_request(void) ; -void receive_position(void) ; -void print_usage(void) ; -void send_labview(char*) ; -void string2data(char*) ; -void* thread_send(void*) ; /* Options */ struct timespec timestamp ; @@ -105,7 +71,7 @@ int main(int argc, char *argv[]) } (void) close(socksendfd) ; (void) close(sockreceivefd) ; - //(void) close(socksendlvfd) ; + //(void) close(socksendlvfd) ; //Use for close labview socket ; for next use return 0 ; } @@ -408,106 +374,150 @@ void receive_position() { // Position of the mobile as computed by the infrastructure: char timestampXYZ[128] ; + char *data_cpy ; recvfrom(sockreceivefd, ×tampXYZ, 128, 0, NULL, NULL) ; - //printf("%s", timestampXYZ) ; - //send_labview(timestampXYZ) ; - string2data(timestampXYZ) ; + //send_labview(timestampXYZ) ; // Use for future + data_cpy = strdup(timestampXYZ) ; + string2data(data_cpy) ; } void string2data(char* string_data) { + /*Découpage de la chaine de caractère reçu du serveur de positionnement + sous forme de addrMAC;TypeRequete;Timestamp1.Timestamp2;Algo;X;Y;Z + en variables séparées + */ + char *mac = NULL ; - int type_req ; - int i ; - int y ; - char *ptr= NULL ; + char *ptr = NULL ; + char *delims = ";" ; - //Lecture Adresse Mac - ptr = strtok(string_data, delims) ; - mac = ptr ; - - //Lecture Type Request - ptr = strtok(NULL, delims) ; - type_req = atoi(ptr) ; + int type_req = 0 ; + int count_algo= 0 ; + int count_print = 0 ; + int onetime = 0 ; + - //Lecture TimeStamp1 - ptr = strtok(NULL, ".") ; - sscanf(ptr, "%ld", ×tamp.tv_sec) ; + while(onetime<1) + { + + //Lecture Adresse Mac + ptr = strtok(string_data, delims) ; + if(ptr==NULL) + { + print_error("mac") ; + break ; + } + mac = ptr ; - //Lecture TimeStamp2 - ptr = strtok(NULL, ";") ; - sscanf(ptr, "%ld", ×tamp.tv_nsec) ; + //Lecture Type Request + ptr = strtok(NULL, delims) ; + if (ptr==NULL) + { + print_error("request") ; + break ; + } + type_req = atoi(ptr) ; + //Lecture TimeStamp1 + ptr = strtok(NULL, ".") ; + if (ptr==NULL) + { + print_error("timestamp") ; + break ; + } + sscanf(ptr, "%ld", ×tamp.tv_sec) ; + + //Lecture TimeStamp2 + ptr = strtok(NULL, ";") ; + if (ptr==NULL) + { + print_error("timestamp") ; + break ; + } + sscanf(ptr, "%ld", ×tamp.tv_nsec) ; + onetime++; + } result results[10]; - i=0; + while(1) { + //Lecture de l'algorythme utilisé ptr = strtok(NULL, delims) ; - if(ptr==NULL) break; - sscanf(ptr, "%s", results[i].algo) ; + if(ptr==NULL) break ; + sscanf(ptr, "%s", results[count_algo].algo) ; + + //Lecture du point X ptr = strtok(NULL, delims) ; - if(ptr==NULL) break; - results[i].x = atof(ptr) ; + if(ptr==NULL) + { + if(count_algo==0) + { + print_error ("trame"); + break ; + } + else + { + print_error ("algo"); + count_algo-- ; + break ; + } + } + results[count_algo].x = atof(ptr) ; + + //Lecture du point Y ptr = strtok(NULL, delims) ; - if(ptr==NULL) break; - results[i].y = atof(ptr) ; + if(ptr==NULL) + { + if(count_algo==0) + { + print_error ("trame"); + break ; + } + else + { + print_error ("algo"); + count_algo--; + break ; + } + } + results[count_algo].y = atof(ptr) ; + + //Lecture du point Z ptr = strtok(NULL, delims) ; - if(ptr==NULL) break; - results[i].z = atof(ptr) ; - i++; + if(ptr==NULL) + { + if(count_algo==0) + { + print_error ("trame"); + break ; + } + else + { + perror ("algo"); + count_algo--; + break ; + } + } + results[count_algo].z = atof(ptr) ; + + count_algo++; + } - for(y=0;y -#define ALGO_STRLEN 15 +#define DEBUG + +/* Lenght of string algo */ +#define ALGO_STRLEN 15 +/* Error codes */ +#define ERR_BAD_USAGE 1 // Bad program call (bad number of arguments) + +/* Number of packets to send */ +#define DEFAULT_NBPKT_CALIB 20 // 20 packets when calibrating +#define DEFAULT_NBPKT_NORMAL 10 // 10 packets when requesting the position + +/* Delay between two packet transmissions (in microseconds) */ +#define DEFAULT_DELAY_CALIB 50000 // Calibration request +#define DEFAULT_DELAY_NORMAL 25000 // Localisation request + +/* Program arguments (getopt string) */ +#define OPTIONS "d:hi:l::n:p:t:" + + +/* Function headers */ +void parse_command_line(int argc, char **argv) ; +void parse_main_options(int argc, char **argv) ; +void check_destination_ip(void) ; +void parse_calibration_data(int argc, char **argv) ; +void check_configuration(void) ; +#ifdef DEBUG +void print_configuration(void) ; +#endif // DEBUG +void create_socket(void) ; +void make_packet(void) ; +void send_request(void) ; +void receive_position(void) ; +void print_usage(void) ; +void send_labview(char*) ; +void string2data(char*) ; +void* thread_send(void*) ; +void print_error(char*) ; typedef struct result result; + +/* Struct */ + struct result { char algo[ALGO_STRLEN];