[ARDrone] Update owlps-drone to multi-algo
Create a array for receive multi-algo. Move all prototypes to .h. Add error message and ignore corrupted trame. Rename variables i->count_algo , y->count_print.
This commit is contained in:
Florian Taillard
parent
9eb3e02b2c
commit
9f77c6c91b
|
|
@ -16,40 +16,6 @@
|
|||
#include <time.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#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<i;y++)
|
||||
|
||||
for(count_print=0;count_print<count_algo;count_print++)
|
||||
{
|
||||
printf("\n----------------\n"
|
||||
" Adresse Mac : %s\n"
|
||||
" Type de requete : %d\n"
|
||||
" Timestamp : %ld . %ld\n"
|
||||
" Timestamp : %ld.%ld\n"
|
||||
" Algo : %s\n"
|
||||
" X : %f\n "
|
||||
" Y : %f\n"
|
||||
" Z : %f\n"
|
||||
"------------------\n", mac, type_req, timestamp.tv_sec, timestamp.tv_nsec, results[y].algo, results[y].x, results[y].y, results[y].z);
|
||||
"------------------\n", mac, type_req, timestamp.tv_sec, timestamp.tv_nsec, results[count_print].algo, results[count_print].x, results[count_print].y, results[count_print].z);
|
||||
}
|
||||
|
||||
//printf("\nsec : %ld nsec : %ld \n", timestamp.tv_sec, timestamp.tv_nsec) ;
|
||||
/*
|
||||
while(1)
|
||||
{
|
||||
switch(i)
|
||||
{
|
||||
|
||||
case 0:
|
||||
ptr = strtok(NULL, delims) ;
|
||||
sscanf(ptr, "%s", &algo) ;
|
||||
i++ ;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
ptr = strtok(NULL, delims) ;
|
||||
sscanf(ptr, "%f", &x) ;
|
||||
i++;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
ptr = strtok(NULL, delims) ;
|
||||
sscanf(ptr, "%f", &y) ;
|
||||
i++;
|
||||
break;
|
||||
|
||||
case 3:
|
||||
ptr = strtok(NULL, delims) ;
|
||||
sscanf(ptr, "%f", &z) ;
|
||||
i++;
|
||||
break;
|
||||
|
||||
default:
|
||||
i++;
|
||||
break;
|
||||
}
|
||||
}
|
||||
*/
|
||||
// printf("\nReceive position.......\n\tTimestamp : %ld.%ld \n\tX : %f \n\tY : %f \n\tZ : %f\n", timestamp.tv_sec, timestamp.tv_nsec, x, y , z) ;
|
||||
}
|
||||
|
||||
void send_labview(char* string_data)
|
||||
|
|
@ -517,6 +527,13 @@ void send_labview(char* string_data)
|
|||
//printf("Send to labView : %s", string_data) ;
|
||||
}
|
||||
|
||||
void print_error(char* merror)
|
||||
{
|
||||
if(!strcmp(merror,"trame")) printf("Impossible à lire la trame : Abandon") ;
|
||||
else if (!strcmp(merror,"algo")) printf("Impossible à lire les coordonnées de l'algo : Abandon") ;
|
||||
else printf("Erreur inconnu : Abandon") ;
|
||||
}
|
||||
|
||||
void print_usage()
|
||||
{
|
||||
printf("Usage:\n"
|
||||
|
|
|
|||
|
|
@ -1,7 +1,46 @@
|
|||
#include <stdio.h>
|
||||
#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];
|
||||
|
|
|
|||
Loading…
Reference in New Issue